//----------------------------------------------------------------------------------------------
//	Filename:	PointCloudSegmentation.h
//	Author:		Sandro Spina
//	Date:		13/02/2013
//----------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------
#include "PointCloudSegmentation.h"
#include "FeatureDescStructureGraph.h"

#include <Shape\CloudPoint.h>
#include <Material\MaterialGroup.h>
#include <System\EngineKernel.h>
#include <Sampler\QuasiRandom.h>
#include <Material\BxDF.h>

#include <boost\lexical_cast.hpp>
#include <boost\interprocess\file_mapping.hpp>
#include <boost\interprocess\mapped_region.hpp>
#include <boost\tokenizer.hpp>
#include <boost\filesystem\path.hpp>
#include <boost\filesystem\operations.hpp>

#include <queue>

using namespace Illumina::Core;

PointCloudSegmentation::PointCloudSegmentation(Environment* p_environment, GeometricPrimitive * p_geometricPrimitive, int p_id, float p_ransacEpsilon)
{
	m_pEnvironment = p_environment;
	m_pEngineKernel = p_environment->GetEngineKernel();	
	m_pointCloud = (KDTreePointCloud*) (p_geometricPrimitive->GetShape());		
	m_id = p_id;
	m_mostVisibileSurface = -1;
	m_maxSurfacesVisibleFromViews = -1;
	m_ransacPlaneFitEpsilon = p_ransacEpsilon;

	m_edgeOcclusions = new std::map<std::pair<int,int>, int>();
	m_featureDescriptionGraphs = new std::vector<FeatureDescStructureGraph*>();

	m_id = m_pointCloud->GetPCSId();

	//if (m_pointCloud->GetNoise() > 0.f) m_pointCloud->AddNoise(m_pointCloud->GetNoise(), 0, m_pointCloud->GetSize());	
	m_pointCloud->Compile();
	m_pointCloud->ComputePointTypes(0, m_pointCloud->GetSize());		

	m_segmentsGraph = GenerateSceneSegmentsGraph(m_pointCloud->GetkNNFlood(), m_pointCloud->GetWorldType());	
	std::vector<int> pruned_points;
	m_segmentsGraph->PruneEdgeSegmentsWithSingleConnection();
	m_segmentsGraph->PruneWeakSurfaceStates(12, pruned_points);	
	MarkPointsAsPruned(pruned_points);
	m_structureGraph = ComputeSceneStructureGraphFromSceneSegmentsGraph(m_segmentsGraph, m_pointCloud->GetWorldType());	
	//m_structureGraph->MergeSmallStates(24, m_segmentsGraph);

	/*Discard points we don't want to view*/	
	if (m_pointCloud->GetPointTypesToPrune().size() > 0)
	{
		for (int i = 0; i<m_pointCloud->GetPointTypesToPrune().size(); i++)
			m_pointCloud->DiscardPointsWithType(m_pointCloud->GetPointTypesToPrune().at(i), 0, m_pointCloud->GetSize());	
		m_pointCloud->Compile();	
	}
	
	ComputeOverlapBetweenConnectedSegmentsOBB();

	//Finally draw the graph to file
	DrawSegmentsGraph(m_segmentsGraph, m_pointCloud->GetName());
	DrawSceneGraph(m_structureGraph, m_pointCloud->GetName(), 1.2f);	
	DrawSceneGraph(m_structureGraph, m_pointCloud->GetName(), 0.1f);	
}

PointCloudSegmentation::PointCloudSegmentation(Environment* p_environment, std::string _path, int p_id, bool &p_loaded)
{
	m_pEnvironment = p_environment;
	m_pEngineKernel = p_environment->GetEngineKernel();
	m_id = p_id;
	m_ransacPlaneFitEpsilon = 0.5f;
	p_loaded = ReadFromBPCSFile(_path);	
	if (p_loaded) m_pointCloud->Compile();
}

PointCloudSegmentation::PointCloudSegmentation(Environment* p_environment, GeometricPrimitive * p_geometricPrimitive, KDTreePointCloud * p_cloud, int p_id, float p_ransacEpsilon)
{
	m_pEnvironment = p_environment;
	m_pEngineKernel = p_environment->GetEngineKernel();		
	m_pointCloud = p_cloud;
	m_id = p_id;

	m_mostVisibileSurface = 0;
	m_maxSurfacesVisibleFromViews = 0;
	m_ransacPlaneFitEpsilon = p_ransacEpsilon;

	m_edgeOcclusions = new std::map<std::pair<int,int>, int>();
	m_featureDescriptionGraphs = new std::vector<FeatureDescStructureGraph*>();

	//if (m_pointCloud->GetNoise() > 0.f) m_pointCloud->AddNoise(m_pointCloud->GetNoise(), 0, m_pointCloud->GetSize());		//Noise should be added by the scanner class
	m_pointCloud->Compile();
	m_pointCloud->ComputePointTypes(0, m_pointCloud->GetSize());		

	//m_segmentationLogsFile.open ("Output/Logs/SegmentationLogs2.txt", std::ios_base::app);
	//m_segmentationLogsFile << "****PCS****" << p_cloud->GetName() << ": #pnts=" << p_cloud->GetSize() << " ****"<< std::endl;
	m_segmentsGraph = GenerateSceneSegmentsGraph(m_pointCloud->GetkNNFlood(), m_pointCloud->GetWorldType());
	std::vector<int> pruned_points;
	m_segmentsGraph->PruneEdgeSegmentsWithSingleConnection();	
	m_segmentsGraph->PruneWeakSurfaceStates(12, pruned_points);
	MarkPointsAsPruned(pruned_points);
	m_structureGraph = ComputeSceneStructureGraphFromSceneSegmentsGraph(m_segmentsGraph, m_pointCloud->GetWorldType());	
	//m_structureGraph->MergeSmallStates(24, m_segmentsGraph);
	//m_segmentationLogsFile.close();

	/*Discard points we don't want to view*/	
	if (m_pointCloud->GetPointTypesToPrune().size() > 0)
	{
		for (int i = 0; i<m_pointCloud->GetPointTypesToPrune().size(); i++)
			m_pointCloud->DiscardPointsWithType(m_pointCloud->GetPointTypesToPrune().at(i), 0, m_pointCloud->GetSize());
		// and re-compile the kd-tree
		m_pointCloud->Compile();	
	}

	ComputeOverlapBetweenConnectedSegmentsOBB();
	
	//Finally draw the graph to file
	DrawSceneGraph(m_structureGraph, m_pointCloud->GetName(), 1.2f);	//This one should discard obb_overlap info since > 1
	DrawSceneGraph(m_structureGraph, m_pointCloud->GetName(), 0.1f);	//This one should print only those edge that overlap by 10%
	DrawSegmentsGraph(m_segmentsGraph, m_pointCloud->GetName());	
}

//Need to ensure that no states are lost in the transition from segment to structure graphs, mainly due to pruning on small states.
StructureGraph * PointCloudSegmentation::ComputeSceneStructureGraphFromSceneSegmentsGraph(SegmentsGraph * segmentsgraph, int p_wt) 
{
	StructureGraph * structuregraph = new StructureGraph(segmentsgraph);
	std::cout << "[StructureGraph Construction] Starting with " << segmentsgraph->m_disjointSegmentsGraphs->size() << " disjoint segments graphs." << std::endl;

	std::vector<SegmentsGraph::SurfaceSegment *> disjoint_graphs;
	std::map<int, SegmentsGraph::SurfaceSegment *> present;
	std::map<int, SegmentsGraph::SurfaceSegment *> not_present;
	std::map<int, SegmentsGraph::SurfaceSegment *> not_present_and_not_fused;
	std::map<int, SegmentsGraph::SurfaceSegment*>::iterator ss_i;

	//Make sure all states are present in disjoint_graphs traversals
	for (std::map<int, SegmentsGraph::SurfaceSegment*>::iterator disjoint_iterator = segmentsgraph->m_disjointSegmentsGraphs->begin(); 
									disjoint_iterator != segmentsgraph->m_disjointSegmentsGraphs->end(); disjoint_iterator++)
	{
		if (segmentsgraph->m_surfaceSegments->find(disjoint_iterator->first) == segmentsgraph->m_surfaceSegments->end())
		{
			//This surface segment got pruned and should be removed from m_disjointSegmentsGraphs.
			//Skip this iteration.
			continue;
		}
		std::vector<int> surfaceSegmentsList = segmentsgraph->BreadthFirstTraversal(disjoint_iterator->second);
		disjoint_graphs.push_back(disjoint_iterator->second);
		for (int j=0; j<surfaceSegmentsList.size(); j++)
		{
			if (present.find(surfaceSegmentsList[j]) == present.end())
			{
				ss_i = segmentsgraph->m_surfaceSegments->find(surfaceSegmentsList[j]);
				if (ss_i != segmentsgraph->m_surfaceSegments->end())
					present[surfaceSegmentsList[j]] = ss_i->second;				
			}
		}		
	}

	for (std::map<int, SegmentsGraph::SurfaceSegment *>::iterator segmentsIterator = segmentsgraph->m_surfaceSegments->begin(); 
		segmentsIterator != segmentsgraph->m_surfaceSegments->end(); segmentsIterator++)
	{
		if (present.find(segmentsIterator->first) == present.end())
		{
			not_present[segmentsIterator->first] = segmentsIterator->second;
		}
	}	

	SegmentsGraph::SurfaceSegment * nxtNotPresent;
	SegmentsGraph::SurfaceSegment * nxtPresent;
	std::map<int, std::vector<SegmentsGraph::SurfaceSegment*>> obb_fusions;
	int contained_in_obb = 0;

	//Check if amongst those surface segments left any can fit within the obbs of surface segments still present
	for (std::map<int, SegmentsGraph::SurfaceSegment *>::iterator not_present_iterator = not_present.begin(); not_present_iterator != not_present.end(); not_present_iterator++)
	{
		nxtNotPresent = not_present_iterator->second;
		bool inOBB = false;
		for (std::map<int, SegmentsGraph::SurfaceSegment *>::iterator present_iterator = present.begin(); present_iterator != present.end(); present_iterator++)
		{
			nxtPresent = present_iterator->second;
			if (segmentsgraph->InOBB(nxtPresent, nxtNotPresent))
			{
				//OBB is a valid container				
				obb_fusions[nxtPresent->id].push_back(nxtNotPresent);
				contained_in_obb++;
				//std::cout << nxtNotPresent->id << "[" << nxtNotPresent->points.size() << "]" << " contained in " << nxtPresent->id << "[" << nxtPresent->points.size() << "]" << std::endl;
				inOBB=true;
				break;
			}
		}
		if (!inOBB) not_present_and_not_fused[nxtNotPresent->id] = nxtNotPresent;
	}
	
	SegmentsGraph::SurfaceSegment * crtSegment;
	std::vector<SegmentsGraph::SurfaceSegment*> fused_segments;
	std::vector<int> * point_indices;
	int points_reused = 0;
	//Assign points in segments contained in segments still present. The obbs of the segments still active will not change.
	for (std::map<int, std::vector<SegmentsGraph::SurfaceSegment*>>::iterator obb_fusions_iterator = obb_fusions.begin(); obb_fusions_iterator != obb_fusions.end(); obb_fusions_iterator++)
	{
		crtSegment = segmentsgraph->m_surfaceSegments->at(obb_fusions_iterator->first);
		fused_segments = obb_fusions_iterator->second;
		for (int i=0; i<fused_segments.size(); i++)
		{
			point_indices = fused_segments[i]->points;
			for (int j=0; j<point_indices->size(); j++)
			{
				crtSegment->points->push_back(point_indices->at(j));
				points_reused++;
			}
		}
	}

	int point_count_missing = 0;
	//Add new disjoint graphs if necessary
	std::map<int, bool> done;
	for (std::map<int, SegmentsGraph::SurfaceSegment *>::iterator still_there_iterator=not_present_and_not_fused.begin(); still_there_iterator!=not_present_and_not_fused.end(); still_there_iterator++)
	{
		point_count_missing += still_there_iterator->second->points->size();
		if (done.find(still_there_iterator->first) != done.end()) continue;  //skip to the next segment. This one has already been included		
		std::vector<int> connected_nodes = segmentsgraph->BreadthFirstTraversal(segmentsgraph->m_surfaceSegments->at(still_there_iterator->first));		
		done[still_there_iterator->first] = true;
		int total = 0;
		for (int i=0; i<connected_nodes.size(); i++) { done[connected_nodes[i]] = true; total+=segmentsgraph->m_surfaceSegments->at(connected_nodes[i])->points->size(); }
		if (total > 12) //If the connected segment has less than 12 points we can't really use it. Just discard it here.
		{
			disjoint_graphs.push_back(segmentsgraph->m_surfaceSegments->at(still_there_iterator->first));
			std::cout << "[SceneGraph Construction] Adding Missing Segment=" << still_there_iterator->first << " with #nodes connected=" << connected_nodes.size()  << ", #points= " << total << std::endl;
		}
	}

	//Output some statistics
	std::cout << "[SceneGraph Construction] Traversed=" << present.size() << ", LeftOut=" << not_present.size() << ", Contained=" << contained_in_obb << ", Containers=" << obb_fusions.size() << std::endl;
	std::cout << "[SceneGraph Construction] Points Reused=" << points_reused << ", SegmentsStillToInclude=" << not_present_and_not_fused.size() << ", PointsStillToInclude=" << point_count_missing << std::endl;		

	
	//replace the elements of m_disjointGraphs with those in disjoint_graphs;
	segmentsgraph->m_disjointSegmentsGraphs->clear();	

	//Create Structure Graph	
	for (int i=0; i<disjoint_graphs.size(); i++)
	{
		std::vector<int> surfaceSegmentsList = segmentsgraph->BreadthFirstTraversal(disjoint_graphs.at(i));
		SegmentsGraph::SurfaceSegment * crtSegment;
		SegmentsGraph::EdgeSegment * crtEdge;

		std::map<int, SegmentsGraph::SurfaceSegment*>::iterator ss_i;

		int initial_state = -1;
		int new_states_count = 0;

		//std::cout << "ADDING TO DISJOINT MAP :: " << disjoint_graphs.at(i)->id << " , #Connections=" << surfaceSegmentsList.size() << std::endl;
		(*(segmentsgraph->m_disjointSegmentsGraphs))[disjoint_graphs.at(i)->id] = disjoint_graphs.at(i);

		//Step 1. Create all nodes
		for (int j=0; j<surfaceSegmentsList.size(); j++)
		{
			ss_i = segmentsgraph->m_surfaceSegments->find(surfaceSegmentsList.at(j));
			if (ss_i == segmentsgraph->m_surfaceSegments->end()) 
			{ 
				std::cout << "[SceneGraph Construction] Skipping surface#" << surfaceSegmentsList.at(j) << ". Not Found!!" << std::endl;
				continue;
			}
			//crtSegment = segmentsgraph->m_surfaceSegments->at(surfaceSegmentsList.at(j));
			if (initial_state == -1) initial_state = ss_i->second->id;
			crtSegment = ss_i->second;
			if (structuregraph->m_nodes->find(crtSegment->id) == structuregraph->m_nodes->end())
			{
				structuregraph->CreateNewNode(crtSegment);
				new_states_count++;
			}
		}

		if (initial_state == -1)
		{
			std::cout << "[SceneGraph Construction] Moving to next disjoint segment. InitialNode = " << initial_state << std::endl;
			continue;
		}

		std::cout << "[SceneGraph Construction] Created " << new_states_count << " states with InitialNode=" << initial_state << std::endl;			

		//Place the first (initial node) in the list of graphs
		structuregraph->m_disjointStructureGraphs->PushBack( structuregraph->m_nodes->find(initial_state)->second );
		//structuregraph->m_disjointStructureGraphs->PushBack(structuregraph->m_nodes->at(surfaceSegmentsList.front()));

		//Step 2. Populate nodes with connectivity information
		for (int j=0; j<surfaceSegmentsList.size(); j++)
		{
			ss_i = segmentsgraph->m_surfaceSegments->find(surfaceSegmentsList.at(j));
			if (ss_i == segmentsgraph->m_surfaceSegments->end()) continue;
			else crtSegment = ss_i->second;				
			for (std::map<int, SegmentsGraph::EdgeSegment*>::iterator edgeSegmentsIterator = crtSegment->connections->begin(); edgeSegmentsIterator != crtSegment->connections->end(); edgeSegmentsIterator++)
			{
				crtEdge = edgeSegmentsIterator->second;
				for (std::map<int, std::pair<int, SegmentsGraph::SurfaceSegment*>>::iterator localSurfaceSegmentsIterator = crtEdge->connections->begin(); localSurfaceSegmentsIterator != crtEdge->connections->end(); localSurfaceSegmentsIterator++)
				{
					if (crtSegment->id != localSurfaceSegmentsIterator->second.second->id) //Do not include self cycles
					{
						//Add new edge, if both surface segments are not pruned and still in m_surfaceSegments map
						if (segmentsgraph->m_surfaceSegments->find(localSurfaceSegmentsIterator->second.second->id) != segmentsgraph->m_surfaceSegments->end())
							structuregraph->AddNodeConnection(crtSegment, localSurfaceSegmentsIterator->second.second, localSurfaceSegmentsIterator->second.first);
					}
				}
			}
		}
		
	}
	return structuregraph;
}
	
//Generate graph in two steps ... first produce segments then connect them into a segments graph
SegmentsGraph * PointCloudSegmentation::GenerateSceneSegmentsGraph(int p_k, int p_wt) 
{
	std::queue<int> currentSegment = std::queue<int>();
	std::queue<int> nextSegments = std::queue<int>();
	int pointCloudSize = m_pointCloud->GetSize();
	int visited = 0;
	std::stringstream mess;
	//::UniqueID segmentID = ::UniqueID();
	Illumina::Core::List<std::map<int, int>> connectionPoints = Illumina::Core::List<std::map<int, int>>();
	std::vector<std::pair<int,int>> connectionPointsList;
	std::vector<std::pair<int,int>> corner_points_adjacent_to_surface_points_vector;
	std::vector<std::pair<int,int>> corner_points_adjacent_to_edge_points_vector;
	std::map<int,std::map<int,int>> corner_segments_adjacent_to_surface_segments_map;	
	std::map<int,std::map<int,int>> corner_segments_adjacent_to_edge_segments_map;	

	SegmentsGraph * graph = new SegmentsGraph(this);
	SegmentsGraph::SurfaceSegment * currentState;
	SegmentsGraph::EdgeSegment * currentEdge;

	const size_t num_results = p_k*2;	 //temporary measure ... for edge points only, extend the search to double the amount of the surface points in order to avoid having small edge segments
	size_t *ret_index = new size_t[num_results];
	float *out_dist_sqr = new float[num_results];	
	float query_pt[3] = { 0.f, 0.f, 0.f };
	int crtPoint = 0;
	int crtType = 0;
	int crtSegmentId = 0;
	int crtConnectionQueue = -1;
	std::map<int, int> pointsPerSegment = std::map<int, int>();

	//**Construct kd-tree
	typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<float, KDTreePointCloud>, KDTreePointCloud, 3 /* dim */> my_kd_tree_t;
	my_kd_tree_t index(3 /*dim*/, *m_pointCloud, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */) );
	index.buildIndex();
	nanoflann::KNNResultSet<float> resultSet(num_results);
	resultSet.init(&ret_index[0], &out_dist_sqr[0]);
	//**Done

	float within_maxdist = m_pointCloud->GetkNNFloodRadius();
	int beyond_maxdist = 0;

	int id_surface = 4; //assuming planar world type
	int id_edge = 3; //assuming planar world type
	int id_corner = 5; //planar world

	if (p_wt==0) id_surface = 1; //switch to line world semantics

	int count_surface_segments = 0;
	int count_edge_segments = 0;
	int count_corner_segments = 0;

	std::vector<int> corner_segments;
	boost::timer floodfill_time;
	//Iterate over point cloud until all points are visited		
	while (visited < pointCloudSize)
	{			
		//Iterate over point cloud to find the next not visited surface point.
		//Get the seed for the next disjoint graph
		bool found = false;			
		for (int i = 0; i<pointCloudSize && !found; i++) 
		{
			//If points lies on a line (LWorld) or a surface (PWorld) use it as seed.
			//if ((pointCloud->m_pointList[i].m_typeId == 1 || pointCloud->m_pointList[i].m_typeId == 4) && pointCloud->m_pointList[i].m_segmentId == -1)
			if (m_pointCloud->m_pointList[i].m_segmentId == -1 && m_pointCloud->m_pointList[i].m_typeId != id_corner)
			{					
				//Before inserting in the nextSegments queue check whether its neighbours have been assigned a segment id
				/*query_pt[0] = pointCloud->m_pointList[crtPoint].m_vertex.Position.X;
				query_pt[1] = pointCloud->m_pointList[crtPoint].m_vertex.Position.Y;
				query_pt[2] = pointCloud->m_pointList[crtPoint].m_vertex.Position.Z;
				resultSet.init(&ret_index[0], &out_dist_sqr[0] );
				index.findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10));
				for (int j=1; j<
				*/

				//Insert seed into currentSegment queue
				nextSegments.push(i);
				found=true;
				connectionPoints.PushBack(std::map<int, int>());
				crtConnectionQueue++;
				//std::cout << "[FloodFill] Grouping points starting at index " << i << ". #boundary points = ";
			}
		}

		if (!found) break; //exit this while loop if a suitable point is not found ... 
			
		//**********************Now generate this graph
		//Iterate over nextSegments queue until this is empty
		const size_t num_results_temp = p_k / 3;
		size_t *ret_index_temp = new size_t[num_results_temp];
		float *out_dist_sqr_temp = new float[num_results_temp];
		nanoflann::KNNResultSet<float> resultSetTemp(num_results_temp);
		std::map<int, float> temp_neighbours_map;
		std::vector<int> temp_neighbours_list;
		std::vector<float> temp_neighbours_dst_list;
		int neighbours_count = 0;

		while (!nextSegments.empty()) 
		{
			if (!((m_pointCloud->m_pointList[nextSegments.front()].m_segmentId)==-1)) 
			{
				nextSegments.pop();
				continue;
			}
			//std::cout << "Starting a new segment at index " << nextSegments.front() << std::endl;
			int nextSeed = nextSegments.front();
			//Assign a new segment id to this point
			m_pointCloud->m_pointList[nextSeed].m_segmentId = graph->segmentID->GetNext();				
			crtSegmentId = m_pointCloud->m_pointList[nextSeed].m_segmentId;
			graph->IncrementPointCountInSegment(crtSegmentId);
			currentSegment.push(nextSeed); //insert in the currentSegment queue for processing
			nextSegments.pop(); //finally remove it from this queue

			if (m_pointCloud->m_pointList[nextSeed].m_typeId == 3) count_edge_segments++;
			if (m_pointCloud->m_pointList[nextSeed].m_typeId == 4) count_surface_segments++;
			if (m_pointCloud->m_pointList[nextSeed].m_typeId == 5) { count_corner_segments++; corner_segments.push_back(crtSegmentId); }
								
			int recomputed = 0;			
			//Finally iterate over currentSegment queue until this is empty								
			while (!currentSegment.empty()) 
			{
				crtPoint = currentSegment.front();								
				crtType = m_pointCloud->m_pointList[crtPoint].m_typeId;				
				//Get the neigbours -- 0: Using standard kNN technique 1: Using more expensive surface crawling				
				if (m_pointCloud->GetkNNFloodMode() == 1)
				{
					query_pt[0] = m_pointCloud->m_pointList[crtPoint].m_vertex.Position.X;
					query_pt[1] = m_pointCloud->m_pointList[crtPoint].m_vertex.Position.Y;
					query_pt[2] = m_pointCloud->m_pointList[crtPoint].m_vertex.Position.Z;
					temp_neighbours_list.clear();
					temp_neighbours_map.clear();							
					resultSetTemp.init(&ret_index_temp[0], &out_dist_sqr_temp[0] );
					index.findNeighbors(resultSetTemp, &query_pt[0], nanoflann::SearchParams(10));				
					temp_neighbours_map[crtPoint] = 0.f;
					for (int i=0; i<num_results_temp; i++) 
					{
						if (out_dist_sqr_temp[i] != 0 && out_dist_sqr_temp[i] < within_maxdist) 
						{
							temp_neighbours_list.push_back(ret_index_temp[i]);
							temp_neighbours_dst_list.push_back(out_dist_sqr_temp[i]);
						}
					}
					for (int i=0; i<temp_neighbours_list.size(); i++) 
					{
							temp_neighbours_map[temp_neighbours_list.at(i)] = temp_neighbours_dst_list.at(i);
							float query_pt[3] = { m_pointCloud->m_pointList[temp_neighbours_list.at(i)].m_vertex.Position[0], 
												m_pointCloud->m_pointList[temp_neighbours_list.at(i)].m_vertex.Position[1], 
												m_pointCloud->m_pointList[temp_neighbours_list.at(i)].m_vertex.Position[2]};
							resultSetTemp.init(&ret_index_temp[0], &out_dist_sqr_temp[0] );
							index.findNeighbors(resultSetTemp, &query_pt[0], nanoflann::SearchParams(10));
							for (int j=0; j<num_results_temp; j++)
							{
								if (temp_neighbours_map.find(ret_index_temp[j]) != temp_neighbours_map.end())
									temp_neighbours_map[ret_index_temp[j]] = out_dist_sqr_temp[j]; //this is value > 0
							}
					}
					int n_count = 0;
					for (std::map<int, float>::iterator ni = temp_neighbours_map.begin(); ni != temp_neighbours_map.end(); ni++)
					{
						if (n_count >= num_results) break;
						ret_index[n_count] = (*ni).first;						
						n_count++;
					}
					neighbours_count = temp_neighbours_map.size();
				} 
				else 
				{
					//Default method
					query_pt[0] = m_pointCloud->m_pointList[crtPoint].m_vertex.Position.X;
					query_pt[1] = m_pointCloud->m_pointList[crtPoint].m_vertex.Position.Y;
					query_pt[2] = m_pointCloud->m_pointList[crtPoint].m_vertex.Position.Z;
					resultSet.init(&ret_index[0], &out_dist_sqr[0] );
					index.findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10));
					if (m_pointCloud->m_pointList[crtPoint].m_typeId == 4)
						neighbours_count = num_results/2;
					else neighbours_count = num_results;
				}

				for (int j=1; j<neighbours_count; j++)
				{
					//std::cout << "Index = " << ret_index[j] << " Dist = " << out_dist_sqr[j] << std::endl;
					if (out_dist_sqr[j] < within_maxdist) //consider only if within a max dist
					{
						if (m_pointCloud->m_pointList[ret_index[j]].m_segmentId == -1) {
							if (m_pointCloud->m_pointList[ret_index[j]].m_typeId == crtType)
							{
								m_pointCloud->m_pointList[ret_index[j]].m_segmentId = crtSegmentId;
								graph->IncrementPointCountInSegment(crtSegmentId);
								currentSegment.push(ret_index[j]);
								//std::cout << "Adding Point " << ret_index[j] << " to Crt Segment " << crtSegmentId << std::endl;
							}
							else
							{
								nextSegments.push(ret_index[j]);
								//We don't need to update state with connectivity of segments with corners.								
								if (m_pointCloud->m_pointList[ret_index[j]].m_typeId != id_corner && m_pointCloud->m_pointList[crtPoint].m_typeId != id_corner)
								{									
									connectionPoints[crtConnectionQueue][crtPoint] = 1;
									connectionPoints[crtConnectionQueue][ret_index[j]] = 1;
									//connectionPointsList.push_back(std::pair<int,int>(crtPoint, ret_index[j])); //Release mode does not give any speed improvements. So stick to maps
								}
								else
								{
									//store corner points separately .. we might need to join corner segments with edge segments.
									if (m_pointCloud->m_pointList[ret_index[j]].m_typeId == id_corner  && m_pointCloud->m_pointList[crtPoint].m_typeId == id_surface) corner_points_adjacent_to_surface_points_vector.push_back(std::pair<int,int>(ret_index[j], crtPoint));
									if (m_pointCloud->m_pointList[ret_index[j]].m_typeId == id_surface && m_pointCloud->m_pointList[crtPoint].m_typeId == id_corner)  corner_points_adjacent_to_surface_points_vector.push_back(std::pair<int,int>(crtPoint, ret_index[j]));
									if (m_pointCloud->m_pointList[ret_index[j]].m_typeId == id_corner  && m_pointCloud->m_pointList[crtPoint].m_typeId == id_edge)	  corner_points_adjacent_to_edge_points_vector.push_back(std::pair<int,int>(ret_index[j], crtPoint)); 
									if (m_pointCloud->m_pointList[ret_index[j]].m_typeId == id_edge    && m_pointCloud->m_pointList[crtPoint].m_typeId == id_corner)  corner_points_adjacent_to_edge_points_vector.push_back(std::pair<int,int>(crtPoint, ret_index[j]));
								}
							}
						}
					}
					else 
					{
						beyond_maxdist++;						
					}

				}
				currentSegment.pop();
				visited++;
			}
			//std::cout << "Recomputed = " << recomputed << std::endl;
			//std::cout << "[Segmentation] New Segment : Type=" << (int) m_pointCloud->m_pointList[crtPoint].m_typeId << " ID=" << crtSegmentId << " Count=" << graph->pointsPerSegment->at(crtSegmentId) << std::endl;			
		}
		
		//std::cout << connectionPoints[crtConnectionQueue].size() << ". Elapsed [" << floodfill_time.elapsed() << "]" << std::endl;
	}

	//**Join edge segments which are only connected through a corner and no surface points.
	int key_c = 0;
	int key_s = 0;
	std::map<int,std::map<int,int>>::iterator it;
	for (int i = 0; i<corner_points_adjacent_to_surface_points_vector.size(); i++)
	{		
		key_c = m_pointCloud->m_pointList[corner_points_adjacent_to_surface_points_vector[i].first].m_segmentId;
		it = corner_segments_adjacent_to_surface_segments_map.find(key_c); 
		if (it != corner_segments_adjacent_to_surface_segments_map.end())
		{
			key_s = m_pointCloud->m_pointList[corner_points_adjacent_to_surface_points_vector[i].second].m_segmentId;
			if ((*it).second.find(key_s)!=(*it).second.end())
			{
				(*it).second[key_s]+=1;
			}
			else
			{
				(*it).second[key_s]=1;
			}
		}
		else
		{
			corner_segments_adjacent_to_surface_segments_map[key_c] = std::map<int,int>();
			key_s = m_pointCloud->m_pointList[corner_points_adjacent_to_surface_points_vector[i].second].m_segmentId;
			corner_segments_adjacent_to_surface_segments_map[key_c][key_s] = 1;
		}
	}

	for (int i = 0; i<corner_points_adjacent_to_edge_points_vector.size(); i++)
	{		
		key_c = m_pointCloud->m_pointList[corner_points_adjacent_to_edge_points_vector[i].first].m_segmentId;
		it = corner_segments_adjacent_to_edge_segments_map.find(key_c); 
		if (it != corner_segments_adjacent_to_edge_segments_map.end())
		{
			key_s = m_pointCloud->m_pointList[corner_points_adjacent_to_edge_points_vector[i].second].m_segmentId;
			if ((*it).second.find(key_s)!=(*it).second.end())
			{
				(*it).second[key_s]+=1;
			}
			else
			{
				(*it).second[key_s]=1;
			}
		}
		else
		{
			corner_segments_adjacent_to_edge_segments_map[key_c] = std::map<int,int>();
			key_s = m_pointCloud->m_pointList[corner_points_adjacent_to_edge_points_vector[i].second].m_segmentId;
			corner_segments_adjacent_to_edge_segments_map[key_c][key_s] = 1;
		}
	}
	
	//**Done
	int total_segments_count = count_surface_segments + count_edge_segments + count_corner_segments;
	std::cout << "[PointCloudSegmentation] Total Segments Created=" << total_segments_count << " S=" << count_surface_segments << " E=" << count_edge_segments << " C=" << count_corner_segments << std::endl;
	//m_segmentationLogsFile << "Total Segments Created=" << total_segments_count << " S=" << count_surface_segments << " E=" << count_edge_segments << " C=" << count_corner_segments << std::endl;

	//Now build the segmentation graph using the connectionPoints queue
	for (int i = 0; i<connectionPoints.Size(); i++)
	{
		std::queue<int> cp = std::queue<int>();
		int surfaceSeed = -1;
		int seedPointsInSegmentCount = 0;
		for (std::map<int, int>::iterator it = connectionPoints[i].begin(); it!=connectionPoints[i].end(); it++)
		{
			cp.push(it->first);
			if (surfaceSeed==-1 && m_pointCloud->m_pointList[it->first].m_typeId==id_surface && graph->PointCountInSegment(m_pointCloud->m_pointList[it->first].m_segmentId) > seedPointsInSegmentCount)
				surfaceSeed = it->first;
		}

		//Create a new PTAGraph for this disjoint part of the point cloud
		if (surfaceSeed==-1) continue;
		SegmentsGraph::SurfaceSegment * initial_state = graph->CreateAddDisjointGraph(m_pointCloud->m_pointList[surfaceSeed].m_segmentId);

		while (!cp.empty())
		{
			int next = cp.front();
			//Get the neigbours
			query_pt[0] = m_pointCloud->m_pointList[next].m_vertex.Position.X;
			query_pt[1] = m_pointCloud->m_pointList[next].m_vertex.Position.Y;
			query_pt[2] = m_pointCloud->m_pointList[next].m_vertex.Position.Z;
			resultSet.init(&ret_index[0], &out_dist_sqr[0]);
			index.findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10));
			
			//check if node with id next already exists. If not create it here.
			if (!graph->SegmentExists(m_pointCloud->m_pointList[next].m_segmentId))
			{
				m_pointCloud->m_pointList[next].m_typeId == id_surface ?	graph->CreateNewSurfaceSegment(m_pointCloud->m_pointList[next].m_segmentId) :					
																			graph->CreateNewEdgeSegment(m_pointCloud->m_pointList[next].m_segmentId);
			}

			//Iterate over neighbouring points and add/update graph components as required
			for (int j=1; j<num_results/2; j++)
			{
				//We are only interested in those points which exhibit a different segmentId and typeId and are not corners
				if (m_pointCloud->m_pointList[next].m_segmentId != m_pointCloud->m_pointList[ret_index[j]].m_segmentId &&
					m_pointCloud->m_pointList[next].m_typeId != m_pointCloud->m_pointList[ret_index[j]].m_typeId &&
					m_pointCloud->m_pointList[ret_index[j]].m_typeId != id_corner) //not corners ... segment graphs describe connectivity between surfaces and edges.
				{
					//first check if current node already exist. If not create.
					if (!graph->SegmentExists(m_pointCloud->m_pointList[ret_index[j]].m_segmentId))
					{
						m_pointCloud->m_pointList[ret_index[j]].m_typeId == id_surface ?	graph->CreateNewSurfaceSegment(m_pointCloud->m_pointList[ret_index[j]].m_segmentId) :
																							graph->CreateNewEdgeSegment(m_pointCloud->m_pointList[ret_index[j]].m_segmentId);
					}
					
					//now update connections and stats
					graph->AddConnection(m_pointCloud->m_pointList[next].m_segmentId, m_pointCloud->m_pointList[ret_index[j]].m_segmentId);
				}
			}
			cp.pop();
		}
	}

	//Update vectors of points in segments.
	int edgepoint_count = 0;
	int surfacepoint_count = 0;
	for (int i = 0; i<pointCloudSize; i++)
	{
		if (m_pointCloud->m_pointList[i].m_typeId == id_surface)
		{
			surfacepoint_count++;
			if (!graph->SurfaceSegmentExists(m_pointCloud->m_pointList[i].m_segmentId))
			{   //A surface segment might not exist simply because it is disconnected from every other segment.
				graph->CreateNewSurfaceSegment(m_pointCloud->m_pointList[i].m_segmentId);
			}
			graph->m_surfaceSegments->at(m_pointCloud->m_pointList[i].m_segmentId)->points->push_back(i);
		}
		else
		{
			if (m_pointCloud->m_pointList[i].m_typeId == id_edge)
			{
				edgepoint_count++;
				if (!graph->EdgeSegmentExists(m_pointCloud->m_pointList[i].m_segmentId))
				{   //A surface segment might not exist simply because it is disconnected from every other segment.
					graph->CreateNewEdgeSegment(m_pointCloud->m_pointList[i].m_segmentId);
				}
				graph->m_edgeSegments->at(m_pointCloud->m_pointList[i].m_segmentId)->points->push_back(i);
			}
		}
	}
	
	//Check for very rough surfaces (or double plane surfaces) which are viewed as one rough surface. e.g. table top or pointcloud in 05_07-17_over_segmentation.blab
	//Use a min number of points as heuristic....check only these edge segments. Only useful to mitigate problems with huge edge segments which should really be surface segments.
	//Also ... do this only if there are more edge points than surface points
	if (m_pointCloud->GetPCSId() != 30000)
	if (edgepoint_count*10 > surfacepoint_count)
	{
//		m_segmentationLogsFile << "Segments Graph #Edges points = " << edgepoint_count << "  #Surfuce points = " << surfacepoint_count << std::endl;
		std::cout << "[EdgeToSurfaceSegmentSwitch START] Min number of points set to " << edgepoint_count*0.1 << std::endl;
		for (std::map<int, SegmentsGraph::EdgeSegment*>::iterator edgeSegmentsIterator = graph->m_edgeSegments->begin();
			edgeSegmentsIterator != graph->m_edgeSegments->end(); edgeSegmentsIterator++)
		{			
			int connected_surfaces_point_count = 0;
			int connected_surfaces_count = 0;

			if (edgeSegmentsIterator->second->points->size() > edgepoint_count*0.1) 
			{
				std::cout << "[BIG --EDGE->"<< edgeSegmentsIterator->first << "-- RanSAC] #e_points=" << edgeSegmentsIterator->second->points->size() << ", 1% of edgepoint_count=" << edgepoint_count*0.01 << ", #conns="<< edgeSegmentsIterator->second->connections->size() << std::endl;
			}

			for (std::map<int, std::pair<int, SegmentsGraph::SurfaceSegment*>>::iterator surf_iterator = edgeSegmentsIterator->second->connections->begin(); 
				surf_iterator != edgeSegmentsIterator->second->connections->end(); surf_iterator++)
			{
				connected_surfaces_point_count+=surf_iterator->second.second->points->size();
				connected_surfaces_count++;
			}			

			//Either this is a big edge with only a few surface points connected to it OR this edge is bigger than 3% of the total number of edge points. 3 is another parameter.
			//if ((edgeSegmentsIterator->second->points.size() > connected_surfaces_point_count) && edgeSegmentsIterator->second->points.size() > edgepoint_count*0.01) 			
			if (edgeSegmentsIterator->second->points->size() > edgepoint_count*0.01) 
			{
				std::cout << "[Applying --EDGE->"<< edgeSegmentsIterator->first << "-- RanSAC] " << "-----------------------NEW EDGE START--------------------------"  << std::endl;
				std::cout << "[Applying --EDGE->"<< edgeSegmentsIterator->first << "-- RanSAC] BEFORE #e_points=" << edgeSegmentsIterator->second->points->size() << ", #conns="<< connected_surfaces_count << ", total s_points= " << connected_surfaces_point_count << std::endl;
				int newSegmentId = graph->CreateNewSurfaceSegment();
				SegmentsGraph::SurfaceSegment * newSurface = graph->m_surfaceSegments->at(newSegmentId);
				std::cout << "[Applying --EDGE->"<< edgeSegmentsIterator->first << "-- RanSAC] Created SURFACE w/segment id " << newSurface->id << std::endl;
				//transfer all points to the new surface segment.
				for (int i = 0; i<edgeSegmentsIterator->second->points->size(); i++) newSurface->points->push_back(edgeSegmentsIterator->second->points->at(i));
				//Apply RANSAC on the new surface.
				mess.str(std::string("")); mess << "[PointCloudSegmentation] RANSAC On Edge->" << edgeSegmentsIterator->first << " BEFORE #e_points=" << edgeSegmentsIterator->second->points->size() << ", #conns="<< connected_surfaces_count << ", total s_points= " << connected_surfaces_point_count;
				(*(m_pEnvironment->GetLogger()))["RANSAC"]->Write(mess.str(), LL_Error);				
				RANSACManager(newSurface, 0.8f, 3000, 0.2f, true);
				//if the new surface can be split do it now .... otherwise connect all connected surfaces to the new surface via new edge segments.
				std::map<int, SegmentsGraph::SurfaceSegment *> newlyCreatedSurfaces;
				std::cout << "Shape_primitive :: " << newSurface->shape_primitive << std::endl;
				if (newSurface->shape_primitive == SegmentsGraph::unknown) continue; //skip this edge. Ransac manager was not able to properly fit a surface.
				if (newSurface->internalSegGraph->m_surfaceSegments->size() > 0)
				{
					std::cout << "[Applying --EDGE->"<< edgeSegmentsIterator->first << "-- RanSAC] Created " << newSurface->internalSegGraph->m_surfaceSegments->size() << " new (interanl) surface segments" << std::endl;
					//Elect the newly created internal states as level 0 states.
					std::map<int, bool> points_used;

					//Create all surfaces first
					for (std::map<int, SegmentsGraph::SurfaceSegment*>::iterator m_surfaceSegmentsIterator = newSurface->internalSegGraph->m_surfaceSegments->begin(); 
						m_surfaceSegmentsIterator != newSurface->internalSegGraph->m_surfaceSegments->end(); m_surfaceSegmentsIterator++)					
					{
						if (m_surfaceSegmentsIterator->second->shape_primitive == SegmentsGraph::plane)
						{
							std::cout << "[Applying --EDGE->"<< edgeSegmentsIterator->first << "-- RanSAC] Internal Surface id " << m_surfaceSegmentsIterator->first << " is plane with " << m_surfaceSegmentsIterator->second->points->size() << " points" << std::endl;
							int newSegmentId = graph->CreateNewSurfaceSegment(SegmentsGraph::plane);							
							SegmentsGraph::SurfaceSegment * level0_surface = graph->m_surfaceSegments->at(newSegmentId);
							level0_surface->orientation = m_surfaceSegmentsIterator->second->orientation;
							std::cout << "[Applying --EDGE->"<< edgeSegmentsIterator->first << "-- RanSAC] New Surface Seg with id " << level0_surface->id << " created. Orientation=" << level0_surface->orientation.ToString() << std::endl;
							int nxtIdx;
							for (int i=0; i<m_surfaceSegmentsIterator->second->points->size(); i++) 
							{
								nxtIdx = m_surfaceSegmentsIterator->second->points->at(i);
								level0_surface->points->push_back(nxtIdx);
								points_used[nxtIdx] = true;
								m_pointCloud->m_pointList[nxtIdx].m_segmentId = level0_surface->id;   //Update Segment ID
								m_pointCloud->m_pointList[nxtIdx].m_typeId = 4;  //Set type to surface
							}
							newlyCreatedSurfaces[m_surfaceSegmentsIterator->first] = level0_surface;
						}
					}

					//Establish Connections of the new surface segments according to RanSacManager
					std::map<std::pair<int,int>, bool> connCreated;
					for (std::map<int, SegmentsGraph::SurfaceSegment*>::iterator m_surfaceSegmentsIterator = newSurface->internalSegGraph->m_surfaceSegments->begin(); 
						m_surfaceSegmentsIterator != newSurface->internalSegGraph->m_surfaceSegments->end(); m_surfaceSegmentsIterator++)					
					{
						if (m_surfaceSegmentsIterator->second->shape_primitive == SegmentsGraph::plane)
						{
							if (newlyCreatedSurfaces.find(m_surfaceSegmentsIterator->first) == newlyCreatedSurfaces.end()) continue;
							SegmentsGraph::SurfaceSegment * crtNewSurface = newlyCreatedSurfaces.find(m_surfaceSegmentsIterator->first)->second;
							for (std::map<int, SegmentsGraph::EdgeSegment*>::iterator connected_edges_iterator = m_surfaceSegmentsIterator->second->connections->begin();
								connected_edges_iterator != m_surfaceSegmentsIterator->second->connections->end(); connected_edges_iterator++)
							{								
								//Grab the edge
								SegmentsGraph::EdgeSegment * connectedEdge = connected_edges_iterator->second;
								for (std::map<int, std::pair<int, SegmentsGraph::SurfaceSegment*>>::iterator connected_surfaces_iterator = connectedEdge->connections->begin();
									connected_surfaces_iterator != connectedEdge->connections->end(); connected_surfaces_iterator++)
								{									
									if (newlyCreatedSurfaces.find(connected_surfaces_iterator->first) != newlyCreatedSurfaces.end())
									{
										int connectedStateId = newlyCreatedSurfaces.find(connected_surfaces_iterator->first)->second->id;
										if (connectedStateId == crtNewSurface->id) continue; //skip this self-state cycle
										std::pair<int,int> con;
										if (connectedStateId < crtNewSurface->id) 
											{ con.first = connectedStateId; con.second = crtNewSurface->id; }
										else 
											{ con.first = crtNewSurface->id; con.second = connectedStateId; }
										if (connCreated.find(con) != connCreated.end()) continue;
										connCreated[con]=true;
										//Create an edge between seg_1 and seg_2
										int newEdgeId = graph->CreateNewEdgeSegment(-1); //-1 will let the graph set the next unique id
										std::cout << "[Applying --EDGE->"<< edgeSegmentsIterator->first << "-- RanSAC] Connecting new Edge#" << newEdgeId << " to " 
											<< crtNewSurface->id << "[" << m_surfaceSegmentsIterator->first << "] with " << connectedStateId << "[" 
											<< connected_surfaces_iterator->first << "]" << std::endl;	
										graph->AddConnection(crtNewSurface->id, newEdgeId);
										graph->AddConnection(connectedStateId, newEdgeId);
									}
								}
							}
						}
					}

					//Compute OBB of the newly created surface segments
					std::map<int, SegmentsGraph::OBB> newlyCreatedSurfacesOBBs;
					for (std::map<int, SegmentsGraph::SurfaceSegment *>::iterator newSurfaces_iterator = newlyCreatedSurfaces.begin(); 
						newSurfaces_iterator != newlyCreatedSurfaces.end(); newSurfaces_iterator++)
					{
						//Set a buffer zone of 1.2f around each OBB. This should be similar to the epsilon set for primitive fitting
						newlyCreatedSurfacesOBBs[newSurfaces_iterator->second->id] = graph->ComputeOBB(newSurfaces_iterator->second->id, 1.2f);
					}

					//Make new connections between the previously connected surface states to the edge turned surface.
					//First Drop all connections between neighbouring surface states to the edge turned surface.
					//All connections should be listed in the connections vector of the edge segment.
					SegmentsGraph::SurfaceSegment *crtSurface;
					std::vector<int> connectedSurfaces;
					std::vector<int> reconnectedSurfaces;  //These surfaces have been rewired and thus their connection should drop
					int allPointsIn = 0;
					for (std::map<int, std::pair<int, SegmentsGraph::SurfaceSegment*>>::iterator connected_surfaces = edgeSegmentsIterator->second->connections->begin();
						connected_surfaces != edgeSegmentsIterator->second->connections->end(); connected_surfaces++)
					{
						connectedSurfaces.push_back(connected_surfaces->first);
					}

					std::map<int, SegmentsGraph::SurfaceSegment*>::iterator m_surfaceSegmentsIterator2;
					
					for (int i=0; i<connectedSurfaces.size(); i++)
					{				
						//std::cout << "%%%%%%%%Fetching " << i << " connected surface " << connectedSurfaces[i] << std::endl;
						if (graph->m_surfaceSegments->find(connectedSurfaces[i]) != graph->m_surfaceSegments->end())
						{
							crtSurface = graph->m_surfaceSegments->at(connectedSurfaces[i]);
						}
						else
						{
							continue; //skip this loop
						}						
						
						//std::cout << "%%%%%%%%Processing Surface Segment " << crtSurface->id << std::endl;
						crtSurface->connections->erase(crtSurface->connections->find(edgeSegmentsIterator->second->id)); //Drop the connections of this surface from the big edge
						for (std::map<int, SegmentsGraph::OBB>::iterator obbs_iterator = newlyCreatedSurfacesOBBs.begin(); obbs_iterator!=newlyCreatedSurfacesOBBs.end(); obbs_iterator++)
						{
							SegmentsGraph::OBB crtOBB = obbs_iterator->second;
							int pointsInObbCount = 0;
							//Check if this surface has any points which reside inside this OBB.
							for (int i2=0; i2<crtSurface->points->size(); i2++)
							{											
								Vector3 test_point_pos = m_pointCloud->m_pointList[crtSurface->points->at(i2)].m_vertex.Position;											
								bool inOBB = false;
								if (crtOBB.p_0_min.GetSide(test_point_pos) == Plane::Side_Positive)
									if (crtOBB.p_0_max.GetSide(test_point_pos) == Plane::Side_Negative)
										if (crtOBB.p_1_min.GetSide(test_point_pos) == Plane::Side_Positive)
											if (crtOBB.p_1_max.GetSide(test_point_pos) == Plane::Side_Negative)
												if (crtOBB.p_2_min.GetSide(test_point_pos) == Plane::Side_Positive)
													if (crtOBB.p_2_max.GetSide(test_point_pos) == Plane::Side_Negative)
														inOBB = true;

								if (inOBB) pointsInObbCount++;
							}
							
							if (pointsInObbCount > 0)
							{
								if (pointsInObbCount == crtSurface->points->size()) 
								{
									allPointsIn++;
									//Therefore no need to add a new connections between states. Simply add these points to the bigger state.
									graph->MergeSurfaceSegments(crtSurface->id, obbs_iterator->first);
									break;
								}
								else
								{
									//Add a New Edge Connection between the states
									int newEdgeId = graph->CreateNewEdgeSegment(-1); //-1 will let the graph set the next unique id
									graph->AddConnection(obbs_iterator->first, newEdgeId);
									graph->AddConnection(crtSurface->id, newEdgeId);
									reconnectedSurfaces.push_back(crtSurface->id);
									std::cout << "[Applying --EDGE->"<< edgeSegmentsIterator->first << "-- RanSAC] New Edge Created Between #" << obbs_iterator->first << " and #" << crtSurface->id << std::endl;
								}
							}
						}											
					}			
					std::cout << "[Applying --EDGE->"<< edgeSegmentsIterator->first << "-- RanSAC] Small Surfaces Merged W/All Points In OBB=" << allPointsIn << std::endl;

					//Remove from the big edge the surfaces which have been reconnected
					int rem = 0;
					for (int i=0; i<reconnectedSurfaces.size(); i++) 
					{
						rem++;
						edgeSegmentsIterator->second->connections->erase(reconnectedSurfaces[i]);
					}
					std::cout << "[Applying --EDGE->"<< edgeSegmentsIterator->first << "-- RanSAC] Surface segments conns removed from edge=" << rem << std::endl;

					//Remove the points from the edge segment which have been assigned to the newly created surfaces.
					std::vector<int> nap;
					int nxtIdx;
					for (int i=0; i<edgeSegmentsIterator->second->points->size(); i++)
					{
						nxtIdx = edgeSegmentsIterator->second->points->at(i);
						if (points_used.find(nxtIdx) == points_used.end()) nap.push_back(nxtIdx);
					}
					edgeSegmentsIterator->second->points->clear();
					for (int i=0; i<nap.size(); i++) edgeSegmentsIterator->second->points->push_back(nap[i]);
					std::cout << "[Applying --EDGE->"<< edgeSegmentsIterator->first << "-- RanSAC] Points removed from Big Edge = " << points_used.size() << std::endl;
					std::cout << "[Applying --EDGE->"<< edgeSegmentsIterator->first << "-- RanSAC] Points still in Big Edge = " << nap.size() << std::endl;
					std::cout << "[Applying --EDGE->"<< edgeSegmentsIterator->first << "-- RanSAC] AFTER #e_points=" << edgeSegmentsIterator->second->points->size() << ", #conns="<< edgeSegmentsIterator->second->connections->size() << std::endl;
				}
				else 
				{
					//RANSAC was not able to fit any primitive shapes.
					//Consider dropping this edge. Or change it to surface segment. Not sure what is best.
					//There are no scenes which need this for now. TODO.
					std::cout << "[Applying --EDGE->"<< edgeSegmentsIterator->first << "-- RanSAC] **** WARNING **** EDGE CANNOT BE SUB-DIVIDED !!!! " << std::endl;
				}				

				//Finally delete the temporarily created surface segment
				graph->m_surfaceSegments->erase(graph->m_surfaceSegments->find(newSegmentId));
			}
		}

		int s_count=0; int e_count=0;
		for (int i=0; i<m_pointCloud->m_pointList.size(); i++)
		{
			if (m_pointCloud->m_pointList[i].m_typeId == 3) e_count++;
			if (m_pointCloud->m_pointList[i].m_typeId == 4) s_count++;
		}

		std::cout << "[EdgeToSurfaceSegmentSwitch END] Segments Graph #Surface Segments = " <<  graph->m_surfaceSegments->size() << "  #Edge Segments = " << graph->m_edgeSegments->size() << std::endl;
		std::cout << "[EdgeToSurfaceSegmentSwitch END] Segments Graph #Surface Points   = " << s_count <<                           "  #Edge points   = " << e_count << std::endl;						
	}

	//Now iterate over all level 0 surface segments and establish primitive shape for this surface, possibly computing  an internal segment graph 
	//Determine Shape (using RANSAC eventually) of points. At the moment we only have lines.

	int compoundCount = 0;
	int planarCount = 0;
	int unknownCount = 0;
	int prunedCount = 0;

	mess.str(std::string("")); mess << "[PointCloudSegmentation] ******************************************\n[PointCloudSegmentation] RANSAC On Level0 Surfaces for PCS "<< m_id << " \n[PointCloudSegmentation] ******************************************\n";
	(*(m_pEnvironment->GetLogger()))["RANSAC"]->Write(mess.str(), LL_Error);

	for (std::map<int, SegmentsGraph::SurfaceSegment*>::iterator surfaceSegmentsIterator = graph->m_surfaceSegments->begin(); 
		surfaceSegmentsIterator != graph->m_surfaceSegments->end(); surfaceSegmentsIterator++)
	{		
		//Compute OBB for this surface segment
		surfaceSegmentsIterator->second->obb = graph->ComputeOBB(surfaceSegmentsIterator->second->id, 0.5f);

		//Compute AABV - 
		float minx, miny, minz; minx = miny = minz = 100000.f;
		float maxx, maxy, maxz; maxx = maxy = maxz = -100000.f;
		for (int j = 0; j < surfaceSegmentsIterator->second->points->size(); j++)
		{
			if (minx > m_pointCloud->m_pointList[surfaceSegmentsIterator->second->points->at(j)].m_vertex.Position.X) minx = m_pointCloud->m_pointList[surfaceSegmentsIterator->second->points->at(j)].m_vertex.Position.X;
			if (miny > m_pointCloud->m_pointList[surfaceSegmentsIterator->second->points->at(j)].m_vertex.Position.Y) miny = m_pointCloud->m_pointList[surfaceSegmentsIterator->second->points->at(j)].m_vertex.Position.Y;
			if (minz > m_pointCloud->m_pointList[surfaceSegmentsIterator->second->points->at(j)].m_vertex.Position.Z) minz = m_pointCloud->m_pointList[surfaceSegmentsIterator->second->points->at(j)].m_vertex.Position.Z;

			if (maxx < m_pointCloud->m_pointList[surfaceSegmentsIterator->second->points->at(j)].m_vertex.Position.X) maxx = m_pointCloud->m_pointList[surfaceSegmentsIterator->second->points->at(j)].m_vertex.Position.X;
			if (maxy < m_pointCloud->m_pointList[surfaceSegmentsIterator->second->points->at(j)].m_vertex.Position.Y) maxy = m_pointCloud->m_pointList[surfaceSegmentsIterator->second->points->at(j)].m_vertex.Position.Y;
			if (maxz < m_pointCloud->m_pointList[surfaceSegmentsIterator->second->points->at(j)].m_vertex.Position.Z) maxz = m_pointCloud->m_pointList[surfaceSegmentsIterator->second->points->at(j)].m_vertex.Position.Z;
		}
		
		//Deploy RANSAC here (only if there are at least 10 points ... check if a primitive type can be set.
		//if (surfaceSegmentsIterator->first == 1)
		//{
		//	std::cout << "Segment 1 :: " << surfaceSegmentsIterator->second->shape_primitive << std::endl;
		//}
		if (surfaceSegmentsIterator->second->points->size() > 12 && surfaceSegmentsIterator->second->shape_primitive == SegmentsGraph::unknown)
			RANSACManager(surfaceSegmentsIterator->second, m_ransacPlaneFitEpsilon, 3000, 0.15f, false);		

		if (surfaceSegmentsIterator->second->shape_primitive == SegmentsGraph::compound) compoundCount++;
		if (surfaceSegmentsIterator->second->shape_primitive == SegmentsGraph::plane) planarCount++;
		if (surfaceSegmentsIterator->second->points->size() <= 12) prunedCount++;
		if (surfaceSegmentsIterator->second->points->size() > 12 && 
			surfaceSegmentsIterator->second->shape_primitive == SegmentsGraph::unknown) unknownCount++;


		surfaceSegmentsIterator->second->bounding_min = Illumina::Core::Vector3(minx, miny, minz);
		surfaceSegmentsIterator->second->bounding_max = Illumina::Core::Vector3(maxx, maxy, maxz);
	}	

	std::cout << "[SegmentsGraph Created] #States=" <<  graph->m_surfaceSegments->size() << " #Edge=" << graph->m_edgeSegments->size() << std::endl;
	std::cout << "[SegmentsGraph Created] #Planar=" <<  planarCount << " #Compound=" << compoundCount << " #Unknown=" << unknownCount << " #Pruned=" << prunedCount << std::endl;

	//Segments have now been created ... and connectivity between edge and surface patches/segments established.		
	//DrawSegmentsGraph(graph);
	
	return graph;
}

//Parameters can be set by user, otherwise heuristics are used to merge states which exhibit similarities
void PointCloudSegmentation::MergeStates(bool _userSet, int _segmentId, int _connectivity, float _angle, float _epsilon, int _level)
{
	if (_userSet)
	{
		if (_level==0)  //Merge level 0 surface segments
		{
			std::vector<int> connected_segments;
			if (_connectivity == 1) 
			{
				connected_segments = m_structureGraph->DirectlyConnectedSegmentsAtAngle(_segmentId, _angle, _epsilon);			
			}
			else
			{
				std::cout << "[UserMerge START] Searching for coplanar segments connected to "<< _segmentId << std::endl;
				connected_segments = m_structureGraph->DirectlyConnectedCoplanarSegments(_segmentId, _angle, _epsilon);			
			}

			if (connected_segments.size() == 0) 
			{
				std::cout << "[UserMerge DONE] Merge ratio = 0. No surface segments are connected to "<< _segmentId << std::endl;
				return;
			}

			//Merge in the biggest state.
			int max_index;
			int max = 0;
			int crt = 0;
			for (int i=0; i<connected_segments.size(); i++)
			{			
				crt = m_segmentsGraph->m_surfaceSegments->at(connected_segments[i])->points->size();
				if (crt > max) 
				{
					max = crt;
					max_index = i;
				}
			}

			std::vector<int> merge_states;
			merge_states.push_back(connected_segments.at(max_index));
			for (int i=0; i<connected_segments.size(); i++)
			{
				if (i!=max_index) merge_states.push_back(connected_segments[i]);
			}
		
			m_structureGraph->MergeStates(merge_states, _level);
		}
		else 
		{
			//Merge internal states in a surface segment and promote to level 0;

		}
	}
	else
	{
		//Auto Merge adjacent co-planar segments.
		int surface_segments_before = m_structureGraph->m_nodes->size();
		float merge_ratio = m_structureGraph->AutoMergeCoplanar(_angle, _epsilon, _level);
		int surface_segments_after = m_structureGraph->m_nodes->size();
		std::cout << "[AutoMergeCoplanar] #nodes before= " << surface_segments_before << ", #nodes after= " << surface_segments_after << std::endl;
	}


	//Produce a new structure graphviz file.
	DrawSceneGraph(m_structureGraph, m_pointCloud->GetName()+"M", 1.2f);
}

//Method used to merge the internal states of a surface segment (which were created using RANSAC)
void PointCloudSegmentation::MergeInternalStates(bool _userSet, SegmentsGraph::SurfaceSegment * _segment, int _connectivity, float _angle, float _epsilon)
{
	//std::map<int, SegmentsGraph::SurfaceSegment*>::iterator parent_iterator = m_segmentsGraph->m_surfaceSegments->find(_segmentId);
	//if (parent_iterator == m_segmentsGraph->m_surfaceSegments->end()) return;
	//SegmentsGraph::SurfaceSegment * parent = m_segmentsGraph->m_surfaceSegments->find(_segmentId)->second;	
	float r = _segment->internalStrGraph->AutoMergeCoplanar(_angle, _epsilon, 1);
	std::cout << "[MergeInternalStates - " <<_segment->id << "] Ratio Merged:" << r << std::endl;
}

void PointCloudSegmentation::MarkPointsAsPruned(std::vector<int> point_indices)
{
	for (int i = 0; i<point_indices.size(); i++)
	{
		m_pointCloud->m_pointList[point_indices[i]].m_typeId = 7;
	}
}

void PointCloudSegmentation::ComputeOverlapBetweenConnectedSegmentsOBB()
{	
	std::stringstream mess;
	mess.str(std::string("")); mess << "[PruneEdgesUsingSegmentOBBs] Starting Edge Pruning by OBB for PCS ID " << m_id;
	(*(m_pEnvironment->GetLogger()))["Coverage"]->Write(mess.str(), LL_Error);
	
	for (auto mp : *(m_structureGraph->m_nodes))
	{	
		for (auto edges : *(mp.second->edges))
		{
			float overlap = m_segmentsGraph->InOBBPercentage(mp.second->segment->obb, edges.second->second->segment);
			edges.second->first->obb_overlap = overlap;
			mess.str(std::string("")); mess << "[PruneEdgesUsingSegmentOBBs] Overlap Between Nodes " << mp.first << " and " << edges.first << " ==  " << overlap;
			(*(m_pEnvironment->GetLogger()))["Coverage"]->Write(mess.str(), LL_Error);
		}
	}

	mess.str(std::string("")); mess << "[Compute Point Coverage] Done for PCS " << m_id;
	(*(m_pEnvironment->GetLogger()))["Coverage"]->Write(mess.str(), LL_Error);
}

void PointCloudSegmentation::ComputePointCoveragePerSegment(int _sampleSize)
{
	std::stringstream mess;
	mess.str(std::string("")); mess << "[Compute Point Coverage] Starting for PCS " << m_id;
	(*(m_pEnvironment->GetLogger()))["Coverage"]->Write(mess.str(), LL_Error);

	float crtPntRadius = m_pointCloud->GetRadius();
	m_pointCloud->SetRadius(0.03f);	
	for (auto mp : *(m_segmentsGraph->m_surfaceSegments))
	{	
		float buffer = mp.second->obb.buffer;		

		OrthonormalBasis basis;
		basis.U = mp.second->obb.obb_2;
		basis.V = mp.second->obb.obb_1;
		basis.W = mp.second->obb.obb_0;

		float U_min = (mp.second->obb.p_2_min.Distance + buffer) * Maths::ISgn(Vector3::Dot(basis.U, mp.second->obb.p_2_min.Normal));
		float U_max = (mp.second->obb.p_2_max.Distance - buffer) * Maths::ISgn(Vector3::Dot(basis.U, mp.second->obb.p_2_max.Normal));
		float U_ivl = U_max - U_min;
		
		float V_min = (mp.second->obb.p_1_min.Distance + buffer) * Maths::ISgn(Vector3::Dot(basis.V, mp.second->obb.p_1_min.Normal));
		float V_max = (mp.second->obb.p_1_max.Distance - buffer) * Maths::ISgn(Vector3::Dot(basis.V, mp.second->obb.p_1_max.Normal));
		float V_ivl = V_max - V_min;

		float W_min = (mp.second->obb.p_0_min.Distance + buffer) * Maths::ISgn(Vector3::Dot(basis.W, mp.second->obb.p_0_min.Normal));
		float W_max = (mp.second->obb.p_0_max.Distance - buffer) * Maths::ISgn(Vector3::Dot(basis.W, mp.second->obb.p_0_max.Normal));
		float z_ = W_max+10.0f; 

		Ray r;

		float hit_count = 0;

		m_pointCloud->SetInvisibleAllPointsExceptForSegmentId(mp.first);		
		
		Vector3 a0 = (U_min * basis.U) + (V_min * basis.V) + (W_min * basis.W);
		Vector3 a1 = (U_min * basis.U) + (V_min * basis.V) + (W_max * basis.W);		
		Vector3 a2 = (U_min * basis.U) + (V_max * basis.V) + (W_min * basis.W);		
		Vector3 a3 = (U_min * basis.U) + (V_max * basis.V) + (W_max * basis.W);

		Vector3 a4 = (U_max * basis.U) + (V_min * basis.V) + (W_min * basis.W);
		Vector3 a5 = (U_max * basis.U) + (V_min * basis.V) + (W_max * basis.W);
		Vector3 a6 = (U_max * basis.U) + (V_max * basis.V) + (W_min * basis.W);
		Vector3 a7 = (U_max * basis.U) + (V_max * basis.V) + (W_max * basis.W);

		mess.str(std::string("")); mess << "[Compute Point Coverage] Segment=" << mp.first << ", OBB=" << basis.ToString();
		//mess << "[Compute Point Coverage] U_min=" << U_min << ", U_max=" << U_max << " :: V_min=" << V_min << ", V_max=" << V_max << " :: W_min=" << W_min << ", W_max=" << W_max;

		/*mess << "MinMinMin::" << a0.ToString() << std::endl;
		mess << "MinMinMax::" << a1.ToString() << std::endl;
		mess << "MinMaxMin::" << a2.ToString() << std::endl;
		mess << "MinMaxMax::" << a3.ToString() << std::endl;
		mess << "MaxMinMin::" << a4.ToString() << std::endl;
		mess << "MaxMinMax::" << a5.ToString() << std::endl;
		mess << "MaxMaxMin::" << a6.ToString() << std::endl;
		mess << "MaxMaxMax::" << a7.ToString() << std::endl;
		*/

		(*(m_pEnvironment->GetLogger()))["Coverage"]->Write(mess.str(), LL_Warning);
		
		for (int sampleIdx = 0; sampleIdx < _sampleSize; sampleIdx++)
		{
			float lds_x = QuasiRandomSequence::VanDerCorput(sampleIdx);
			float lds_y = QuasiRandomSequence::Sobol2(sampleIdx);

			float x_ = (lds_x * U_ivl) + U_min;
			float y_ = (lds_y * V_ivl) + V_min;
			
			r.Set( x_*basis.U + y_*basis.V + z_*basis.W, -basis.W);			

			bool hit = m_pointCloud->Intersects(r);
			mess.str(std::string("")); mess << "[Compute Point Coverage] S(" <<  sampleIdx << ") Ray=" << r.ToString() << " [" << hit <<"]";
			(*(m_pEnvironment->GetLogger()))["Coverage"]->Write(mess.str(), LL_Info);
			if (hit) hit_count++;
		}

		mp.second->coverage = hit_count/_sampleSize;
		mess.str(std::string("")); mess << "[Compute Point Coverage] Segment=" << mp.first << ", Hit Count = " << hit_count << ", Coverage = " << mp.second->coverage;
		(*(m_pEnvironment->GetLogger()))["Coverage"]->Write(mess.str(), LL_Warning);
	}

	//Set all point visible before leaving and restore radius of points;
	m_pointCloud->SetVisibleAllPoints();
	m_pointCloud->SetRadius(crtPntRadius);

	mess.str(std::string("")); mess << "[Compute Point Coverage] Done for PCS " << m_id;
	(*(m_pEnvironment->GetLogger()))["Coverage"]->Write(mess.str(), LL_Error);
}

//Compute OBB for the whole point cloud
void PointCloudSegmentation::ComputeOBB(float _buffer)
{	
	m_obb.buffer = _buffer;
	float c00, c11, c22, c01, c02, c12;
	c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;						
	Eigen::Matrix3f covariance = Eigen::Matrix3f();				
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver;
	Vector3 median = Vector3(0);
		
	int numOfPoints = m_pointCloud->m_pointList.size();

	for (int j=0; j<numOfPoints; j++)
	{					
		median.X += m_pointCloud->m_pointList[j].m_vertex.Position[0];
		median.Y += m_pointCloud->m_pointList[j].m_vertex.Position[1];
		median.Z += m_pointCloud->m_pointList[j].m_vertex.Position[2];							
	}
	median /= numOfPoints;

	//Now calculate diagonal of covariance matrix
	for (int j=0; j<numOfPoints; j++) 
	{			
		c00 += (m_pointCloud->m_pointList[j].m_vertex.Position[0] - median.X) * (m_pointCloud->m_pointList[j].m_vertex.Position[0] - median.X);
		c11 += (m_pointCloud->m_pointList[j].m_vertex.Position[1] - median.Y) * (m_pointCloud->m_pointList[j].m_vertex.Position[1] - median.Y);
		c22 += (m_pointCloud->m_pointList[j].m_vertex.Position[2] - median.Z) * (m_pointCloud->m_pointList[j].m_vertex.Position[2] - median.Z);
		c01 += (m_pointCloud->m_pointList[j].m_vertex.Position[0] - median.X) * (m_pointCloud->m_pointList[j].m_vertex.Position[1] - median.Y);
		c02 += (m_pointCloud->m_pointList[j].m_vertex.Position[0] - median.X) * (m_pointCloud->m_pointList[j].m_vertex.Position[2] - median.Z);
		c12 += (m_pointCloud->m_pointList[j].m_vertex.Position[1] - median.Y) * (m_pointCloud->m_pointList[j].m_vertex.Position[2] - median.Z);		
	}				

	covariance << c00 / numOfPoints, c01 / numOfPoints, c02 / numOfPoints, 
				c01 / numOfPoints, c11 / numOfPoints, c12 / numOfPoints, 
				c02 / numOfPoints, c12 / numOfPoints, c22 / numOfPoints;
				
	eigensolver.compute(covariance);			
	//std::cout << "[Eiegenvectors] " << eigensolver.eigenvectors().col(0) << " " << eigensolver.eigenvectors().col(1) << " " << eigensolver.eigenvectors().col(2) << " " << std::endl; 

	m_obb.obb_0 = Vector3(eigensolver.eigenvectors().col(0)[0], eigensolver.eigenvectors().col(0)[1], eigensolver.eigenvectors().col(0)[2]);				
	m_obb.obb_1 = Vector3(eigensolver.eigenvectors().col(1)[0], eigensolver.eigenvectors().col(1)[1], eigensolver.eigenvectors().col(1)[2]);
	m_obb.obb_2 = Vector3(eigensolver.eigenvectors().col(2)[0], eigensolver.eigenvectors().col(2)[1], eigensolver.eigenvectors().col(2)[2]);

	//Now move all the points in the coordinate space of the OBB
	std::vector<Vector3> OBBpoints;
	for (int i=0; i<numOfPoints; i++) OBBpoints.push_back(Vector3(Vector3::Dot(m_pointCloud->m_pointList[i].m_vertex.Position, m_obb.obb_0), 
																	Vector3::Dot(m_pointCloud->m_pointList[i].m_vertex.Position, m_obb.obb_1), 
																	Vector3::Dot(m_pointCloud->m_pointList[i].m_vertex.Position, m_obb.obb_2)));

	float obb_0_d_min = 1000000.0f; float obb_0_d_max = -1000000.0f;
	float obb_1_d_min = 1000000.0f; float obb_1_d_max = -1000000.0f;
	float obb_2_d_min = 1000000.0f; float obb_2_d_max = -1000000.0f;

	for (int i=0; i<OBBpoints.size(); i++) 
	{
		if (OBBpoints[i].X > obb_0_d_max) obb_0_d_max = OBBpoints[i].X;
		if (OBBpoints[i].Y > obb_1_d_max) obb_1_d_max = OBBpoints[i].Y;
		if (OBBpoints[i].Z > obb_2_d_max) obb_2_d_max = OBBpoints[i].Z;

		if (OBBpoints[i].X < obb_0_d_min) obb_0_d_min = OBBpoints[i].X;
		if (OBBpoints[i].Y < obb_1_d_min) obb_1_d_min = OBBpoints[i].Y;
		if (OBBpoints[i].Z < obb_2_d_min) obb_2_d_min = OBBpoints[i].Z;
	}

	m_obb.p_0_min = Plane(m_obb.obb_0, obb_0_d_min - _buffer);
	m_obb.p_0_max = Plane(m_obb.obb_0, obb_0_d_max + _buffer);
	m_obb.p_1_min = Plane(m_obb.obb_1, obb_1_d_min - _buffer);
	m_obb.p_1_max = Plane(m_obb.obb_1, obb_1_d_max + _buffer);
	m_obb.p_2_min = Plane(m_obb.obb_2, obb_2_d_min - _buffer);
	m_obb.p_2_max = Plane(m_obb.obb_2, obb_2_d_max + _buffer);	
}

//Compute Feature Structure Graphs Matrix
bool PointCloudSegmentation::ComputeFeatureMatrix(int _featureGraphsCount, int _depth, float p_cell_length)
{
	//Enumerate surface segments according to point count and coverage ratios using Equation PointCloud * Coverage (Normalised).
	std::map<int, int> segmentsByPointSize;
	std::map<float, int> segmentsByCoverage;
	std::map<float, int> segmentsByPointSizeAndCoverage;
	std::stringstream mess;
	mess.str(std::string("")); mess << "[Compute Feature Structures] Starting for PCS " << m_id << " #States=" << m_segmentsGraph->m_surfaceSegments->size();
	(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Critical);
	
	for (auto mp : *(m_structureGraph->m_nodes))
	{	
		segmentsByPointSize[(m_segmentsGraph->m_surfaceSegments->at(mp.first))->points->size()] = mp.second->id;
		segmentsByCoverage[(m_segmentsGraph->m_surfaceSegments->at(mp.first))->coverage] = mp.second->id;
		segmentsByPointSizeAndCoverage[(m_segmentsGraph->m_surfaceSegments->at(mp.first))->coverage * (m_segmentsGraph->m_surfaceSegments->at(mp.first))->points->size()] = mp.second->id;		
	}

	mess.str(std::string("")); mess << "[Compute Feature Structures] Surface Segments Ordered By Point Count\n-------------------------------";
	(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Info);
	
	//Iterators should give me everything ordered by key
	for (auto sps : segmentsByPointSize)
	{			
		mess.str(std::string("")); mess << "[Compute Feature Structures] #Points::" << sps.first << " -> ID::" << sps.second;
		(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Info);
	}

	mess.str(std::string("")); mess << "[Compute Feature Structures] Surface Segments Ordered By Coverage\n-------------------------------";
	(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Info);	

	//Iterators should give me everything ordered by key
	for (auto sps : segmentsByCoverage)
	{			
		mess.str(std::string("")); mess << "[Compute Feature Structures] #Coverage::" << sps.first << " -> ID::" << sps.second;
		(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Info);
	}

	mess.str(std::string("")); mess << "[Compute Feature Structures] Surface Segments Ordered By CoverageCrossSize\n-------------------------------";
	(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Info);	
	
	std::vector<StructureGraph::Node *> initStates;

	//Iterators should give me everything ordered by key
	for (auto sps : segmentsByPointSizeAndCoverage)
	{			
		mess.str(std::string("")); mess << "[Compute Feature Structures] #Points_X_Coverage::" << sps.first << " -> ID::" << sps.second;
		(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Error);
		initStates.insert(initStates.begin(), m_structureGraph->m_nodes->at(sps.second));
	}	

	//----- Important Segments Chosen - Now build the FeatureDescriptorStructureGraphs ------

	//Create FeatureDescriptorStructureGraphs with these three initial states - max depth == 3
	mess.str(std::string(""));
	for (int i = 0; i<_featureGraphsCount; i++)
	{
		if (i > initStates.size() - 1) break;
		mess << "[Compute Feature Structures] HIGHEST ("<< i << ") COVERAGE X SIZE Surface ID:: " <<  initStates[i]->id << ", Score::" <<
			m_segmentsGraph->m_surfaceSegments->at(initStates[i]->id)->coverage * m_segmentsGraph->m_surfaceSegments->at(initStates[i]->id)->points->size() << std::endl;
		FeatureDescStructureGraph * feature_graph = new FeatureDescStructureGraph(initStates[i]->id, m_segmentsGraph->m_pcs);
		FeatureDescStructureGraph::Node * fd_root_node = feature_graph->GetNode(initStates[i]->id);
		feature_graph->CopyProperties(fd_root_node, initStates[i]);
		m_featureDescriptionGraphs->push_back(feature_graph);
		std::vector<int> crt_level; crt_level.push_back(initStates[i]->id);
		std::map<int,int> nxt_level;
		int depth = _depth;
		while (depth > 0)
		{
			for (int j = 0; j<crt_level.size(); j++)
			{				
				StructureGraph::Node * crt_node = m_structureGraph->m_nodes->at(crt_level[j]);
				FeatureDescStructureGraph::Node * fd_src_node = feature_graph->GetNode(crt_level[j]);
				//Grab edges connected to this crt_node
				for (std::map<int, std::pair<StructureGraph::EdgeStats*, StructureGraph::Node*> *>::iterator edge_it = crt_node->edges->begin(); edge_it != crt_node->edges->end(); edge_it++)
				{
					if (edge_it->second->first->obb_overlap == 0) continue;   //if there is no obb overlap between these segments simply move on.
					StructureGraph::Node * sink_node = edge_it->second->second;
					FeatureDescStructureGraph::Node * fd_sink_node = feature_graph->AddNode(sink_node);
					//Add a connection between these two segments
					feature_graph->AddConnection(fd_src_node, fd_sink_node, edge_it->second->first);
					nxt_level[fd_sink_node->id] = 1;
				}
			}
			depth = depth-1;
			crt_level.clear();
			for (std::map<int,int>::iterator nxt_it = nxt_level.begin(); nxt_it != nxt_level.end(); nxt_it++)
			{
				crt_level.push_back(nxt_it->first);
			}
		}
		feature_graph->ComputeStringDescriptors(5);
		std::cout << " ********* " << std::endl;
		feature_graph->ComputeVoxelGrids(p_cell_length, m_pointCloud->m_pointList);
		std::cout << " ********* " << std::endl;
	}

	//Write Log 
	(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Critical);
	mess.str(std::string(""));	

	//Also - Create a FeatureDescriptorStructureGraph from the most visible surface segment - TODO

	PrintOptimalFeatureStrings();

	return true;
}

void PointCloudSegmentation::AugmentVoxelGridWithEdgeSegments(FeatureDescStructureGraph::VoxelGrid * voxel_grid, std::vector<int> p_neighbourEdgeSegments)
{
	SegmentsGraph::EdgeSegment * crtEdgeSegment;
	CloudPoint crtPoint(1,1,1);

	Vector3 p_translated;
	Vector3 p_uvw_r;	
	bool out = false;

	Vector3 o_m_xyz = voxel_grid->o_model_xyz;
	float g_c_s_u = 1/voxel_grid->grid_cell_spacing_u;
	float g_c_s_v = 1/voxel_grid->grid_cell_spacing_v;
	float g_c_s_w = 1/voxel_grid->grid_cell_spacing_w;

	Vector3 uvwU = voxel_grid->uvw.U;
	Vector3 uvwV = voxel_grid->uvw.V;
	Vector3 uvwW = voxel_grid->uvw.W;
	int u_c_c = voxel_grid->u_cells_count;
	int v_c_c = voxel_grid->v_cells_count;
	int w_c_c = voxel_grid->w_cells_count;

	int previous_d_u_cell = -1;
	int previous_d_v_cell = -1;
	int previous_d_w_cell = -1;

	int d_u_cell;
	int d_v_cell;
	int d_w_cell;

	float d_u_cell_float = 0.0f;
	float d_v_cell_float = 0.0f;
	float d_w_cell_float = 0.0f;

	Vector3 mm_uvw; 	

	mm_uvw.X = voxel_grid->min_model_uvw_br.X;
	mm_uvw.Y = voxel_grid->min_model_uvw_br.Y;
	mm_uvw.Z = voxel_grid->min_model_uvw_br.Z;

	Matrix3x3 yz_rotation_i = Matrix3x3::CreateRotation(voxel_grid->uvw.U, -(voxel_grid->rotation));

	int original_number_of_voxels = voxel_grid->m_voxels->size();
	int added_points = 0;
	for (auto edgeSegId : p_neighbourEdgeSegments)
	{
		crtEdgeSegment = m_segmentsGraph->EdgeSegmentWithId(edgeSegId);
		for (auto vertexId : (*crtEdgeSegment->points))
		{
			crtPoint = m_pointCloud->m_pointList.at(vertexId);															
			p_translated = crtPoint.m_vertex.Position - voxel_grid->o_model_xyz;
			Matrix3x3::Product(yz_rotation_i, p_translated, p_uvw_r);		

			d_u_cell_float = (p_uvw_r.Dot(uvwU) - mm_uvw.X) * g_c_s_u;			

			if (d_u_cell_float < -1) { out=true; }
			else
			{
				d_u_cell = (int) d_u_cell_float;
				if (d_u_cell_float < 0) d_u_cell = 0;
				if (d_u_cell > u_c_c) { out = true; }
			}

			d_v_cell_float = (p_uvw_r.Dot(uvwV) - mm_uvw.Y) * g_c_s_v;
			if (d_v_cell_float < 0) { out=true; } 
			else
			{
				d_v_cell = (int) d_v_cell_float;
				if (d_v_cell > v_c_c) { out = true; }
			}

			d_w_cell_float = (p_uvw_r.Dot(uvwW) - mm_uvw.Z) * g_c_s_w;
			if (d_w_cell_float < 0) { out=true; } 
			else
			{
				d_w_cell = (int) d_w_cell_float;
				if (d_w_cell > w_c_c) { out = true; }
			}		
			
			if (!out) 
			{
				added_points++;
				if (d_u_cell == previous_d_u_cell)
					if (d_v_cell == previous_d_v_cell)
						if (d_w_cell == previous_d_w_cell)
							voxel_grid->IncActiveVoxelEdgeCount();
						else
						{
							voxel_grid->IncVoxelEdgeCount(d_u_cell, d_v_cell, d_w_cell, edgeSegId);
							previous_d_u_cell = d_u_cell;
							previous_d_v_cell = d_v_cell;
							previous_d_w_cell = d_w_cell;
						}
					else
					{
						voxel_grid->IncVoxelEdgeCount(d_u_cell, d_v_cell, d_w_cell, edgeSegId);
						previous_d_u_cell = d_u_cell;
						previous_d_v_cell = d_v_cell;
						previous_d_w_cell = d_w_cell;
					}
				else
				{
					voxel_grid->IncVoxelEdgeCount(d_u_cell, d_v_cell, d_w_cell, edgeSegId);
					previous_d_u_cell = d_u_cell;
					previous_d_v_cell = d_v_cell;
					previous_d_w_cell = d_w_cell;
				}
			}
			out=false;
		}	
	}
	int final_number_of_voxels = voxel_grid->m_voxels->size();
	//std::cout << "Added points count :: " << added_points << " in " << final_number_of_voxels - original_number_of_voxels << " voxels" << std::endl;
}

void PointCloudSegmentation::ComputeVoxelGridAroundDownsampledSegment(std::vector<int> p_segmentIds, FeatureDescStructureGraph::VoxelGrid * p_vg, int p_gridLod, float _rotationVWPlane, 
																					Vector3 &min_uvw_translate, Vector3 &max_uvw_translate, FeatureDescStructureGraph::VoxelGrid * _vgrid, std::map<int, std::vector<int>*> * p_downsampledSegmnets)
{
	_vgrid->m_voxels->clear();
	_vgrid->u_cells_count = p_vg->u_cells_count;
	_vgrid->v_cells_count = p_vg->v_cells_count;
	_vgrid->w_cells_count = p_vg->w_cells_count;
	
	_vgrid->o_grid_u = p_vg->o_grid_u; 
	_vgrid->o_grid_v = p_vg->o_grid_v;	
	_vgrid->o_grid_w = p_vg->o_grid_w;

	if (p_gridLod!=1)
	{
		_vgrid->u_cells_count /= p_gridLod;
		_vgrid->v_cells_count /= p_gridLod;
		_vgrid->w_cells_count /= p_gridLod;
	
		_vgrid->o_grid_u /= p_gridLod; 
		_vgrid->o_grid_v /= p_gridLod;	
		_vgrid->o_grid_w /= p_gridLod;
	}
					
	SegmentsGraph::SurfaceSegment * p_node = m_structureGraph->m_nodes->at(p_segmentIds.front())->segment;
	int p_node_id = p_node->id;

	_vgrid->o_model_xyz = (p_node->obb.p_0_min.Distance + ((p_node->obb.p_0_max.Distance - p_node->obb.p_0_min.Distance) * 0.5f)) * (p_node->obb.obb_0) +
						(p_node->obb.p_1_min.Distance + ((p_node->obb.p_1_max.Distance - p_node->obb.p_1_min.Distance) * 0.5f)) * (p_node->obb.obb_1) +
						(p_node->obb.p_2_min.Distance + ((p_node->obb.p_2_max.Distance - p_node->obb.p_2_min.Distance) * 0.5f)) * (p_node->obb.obb_2);

	_vgrid->uvw.U = p_node->obb.obb_0;

	Matrix3x3 yz_rotation = Matrix3x3::CreateRotation(_vgrid->uvw.U, _rotationVWPlane);
	_vgrid->rotation = _rotationVWPlane;

	_vgrid->uvw.V = p_node->obb.obb_1;
	_vgrid->uvw.W = p_node->obb.obb_2;
	
	float min_u = 100000.f;
	float min_v = 100000.f;
	float min_w = 100000.f;

	float max_u = -100000.f;
	float max_v = -100000.f;
	float max_w = -100000.f;

	float d_u; float d_v; float d_w;

	//std::vector<Vector3> obb_corners = p_node->obb.GetOBBCorners();	
	Vector3 p_tr;
	for (auto corners : p_node->obb.GetOBBCorners())
	{
		p_tr = corners - _vgrid->o_model_xyz;		
		d_u = p_tr.Dot(_vgrid->uvw.U);
		d_v = p_tr.Dot(_vgrid->uvw.V);
		d_w = p_tr.Dot(_vgrid->uvw.W);
		if (d_u > max_u) max_u = d_u; if (d_u < min_u) min_u = d_u;
		if (d_v > max_v) max_v = d_v; if (d_v < min_v) min_v = d_v;
		if (d_w > max_w) max_w = d_w; if (d_w < min_w) min_w = d_w;
	}	

	_vgrid->min_obb_uvw = Vector3(min_u, min_v, min_w) - min_uvw_translate;
	_vgrid->max_obb_uvw = Vector3(max_u, max_v, max_w) + max_uvw_translate;
	
	Vector3 obb_span_model_segment = p_vg->max_obb_uvw - p_vg->min_obb_uvw;
	Vector3 obb_span_scene_segment = _vgrid->max_obb_uvw - _vgrid->min_obb_uvw;

	obb_span_model_segment.X = Maths::Max(obb_span_model_segment.X, 0.8f);

	//std::cout << "Original OBB Min=" << Vector3(min_u, min_v, min_w).ToString() << " " << Vector3(max_u, max_v, max_w).ToString() << std::endl;
	//std::cout << "Increasing OBB to :: " << _vgrid->min_obb_uvw.ToString() << " " << _vgrid->max_obb_uvw.ToString() << std	::endl;
	//std::cout << "OBB Span Model:: " << obb_span_model_segment.ToString() << std::endl;

	//Scale (non uniform) is set as the ratio betwween the U,V,W spans of the scene obb and the model obb.
	float scale_factor_u = (obb_span_scene_segment.X / obb_span_model_segment.X) * p_gridLod;
	float scale_factor_v = (obb_span_scene_segment.Y / obb_span_model_segment.Y) * p_gridLod;
	float scale_factor_w = (obb_span_scene_segment.Z / obb_span_model_segment.Z) * p_gridLod;

	Vector3 scale = Vector3(scale_factor_u, scale_factor_v, scale_factor_w);

	//std::cout << "Scaling Factor (with LOD factor) :: " << scale_factor_u  << "," << scale_factor_v << "," << scale_factor_w << std::endl;

	_vgrid->grid_cell_spacing_u = p_vg->grid_cell_spacing_u * scale_factor_u;
	_vgrid->grid_cell_spacing_v = p_vg->grid_cell_spacing_v * scale_factor_v;
	_vgrid->grid_cell_spacing_w = p_vg->grid_cell_spacing_w * scale_factor_w;
	
	//std::cout << "GridCell Spacing :: " << _vgrid->grid_cell_spacing_u << " " << _vgrid->grid_cell_spacing_v << " " << _vgrid->grid_cell_spacing_w << std::endl;

	Vector3 min_m_xyz_ = p_vg->min_model_uvw * scale;
	Vector3 max_m_xyz_ = p_vg->max_model_uvw * scale;

	//min_m_xyz_.X = Maths::Min(min_m_xyz_.X, -0.1f);

	//std::cout << "Min_Model UVW:: " << p_vg->min_model_uvw.ToString() << " MaxModel UVW:: " << p_vg->max_model_uvw.ToString() << std::endl;
	//std::cout << "Min_Model UVW (Scaled):: " << min_m_xyz_.ToString() << " MaxModel UVW (Scaled):: " << max_m_xyz_.ToString() << std::endl;
	
	Vector3 min_m_xyz; Vector3 max_m_xyz;
	Matrix3x3 uvw_inverse = _vgrid->uvw.GetMatrix(); uvw_inverse.Transpose();					
						
	Matrix3x3::Product(uvw_inverse, min_m_xyz_, min_m_xyz);
	Matrix3x3::Product(uvw_inverse, max_m_xyz_, max_m_xyz);
	
	Vector3 min_model_xyz_rot;
	Vector3 max_model_xyz_rot;	

	Vector3 mm_uvw; 
	mm_uvw.X = min_m_xyz.Dot(_vgrid->uvw.U);
	mm_uvw.Y = min_m_xyz.Dot(_vgrid->uvw.V);
	mm_uvw.Z = min_m_xyz.Dot(_vgrid->uvw.W);

	//std::cout << "Min Model XYZ: " << min_m_xyz.ToString() << " Max Model XYZ: " << max_m_xyz.ToString() << "Min Model UVW: " << mm_uvw.ToString() << std::endl;	

	_vgrid->min_model_uvw_br.X = mm_uvw.X;
	_vgrid->min_model_uvw_br.Y = mm_uvw.Y;
	_vgrid->min_model_uvw_br.Z = mm_uvw.Z;
		
	Matrix3x3::Product(yz_rotation, min_m_xyz, min_model_xyz_rot);
	Matrix3x3::Product(yz_rotation, max_m_xyz, max_model_xyz_rot);

	_vgrid->min_model_uvw.X = min_model_xyz_rot.Dot(_vgrid->uvw.U);
	_vgrid->min_model_uvw.Y = min_model_xyz_rot.Dot(_vgrid->uvw.V);
	_vgrid->min_model_uvw.Z = min_model_xyz_rot.Dot(_vgrid->uvw.W);
	_vgrid->max_model_uvw.X = max_model_xyz_rot.Dot(_vgrid->uvw.U);
	_vgrid->max_model_uvw.Y = max_model_xyz_rot.Dot(_vgrid->uvw.V);
	_vgrid->max_model_uvw.Z = max_model_xyz_rot.Dot(_vgrid->uvw.W);
	//std::cout << "NEW_VG MinUVW::" << new_vg->min_model_uvw.ToString() << " MaxUVW::" << new_vg->max_model_uvw.ToString() << std::endl;
		
	int d_u_cell;
	int d_v_cell;
	int d_w_cell;

	int points_outside_grid_u_pos = 0;
	int points_outside_grid_u_neg = 0;
	int points_outside_grid_v_pos = 0;
	int points_outside_grid_v_neg = 0;
	int points_outside_grid_w_pos = 0;
	int points_outside_grid_w_neg = 0;

	int dist_outside_grid_u_pos = 0;
	int dist_outside_grid_u_neg = 0;
	int dist_outside_grid_v_pos = 0;
	int dist_outside_grid_v_neg = 0;
	int dist_outside_grid_w_pos = 0;
	int dist_outside_grid_w_neg = 0;

	Vector3 p_uvw;
	Vector3 p_translated;
	Vector3 p_uvw_r;	
	Matrix3x3 uvw_m = _vgrid->uvw.GetMatrix();
	
	Matrix3x3 yz_rotation_i = Matrix3x3::CreateRotation(_vgrid->uvw.U, -_rotationVWPlane);
	CloudPoint p = CloudPoint(0,0,0);
	bool out = false;

	Vector3 o_m_xyz = _vgrid->o_model_xyz;
	float g_c_s_u = 1/_vgrid->grid_cell_spacing_u;
	float g_c_s_v = 1/_vgrid->grid_cell_spacing_v;
	float g_c_s_w = 1/_vgrid->grid_cell_spacing_w;

	Vector3 uvwU = _vgrid->uvw.U;
	Vector3 uvwV = _vgrid->uvw.V;
	Vector3 uvwW = _vgrid->uvw.W;
	int u_c_c = _vgrid->u_cells_count;
	int v_c_c = _vgrid->v_cells_count;
	int w_c_c = _vgrid->w_cells_count;

	int previous_d_u_cell = -1;
	int previous_d_v_cell = -1;
	int previous_d_w_cell = -1;	

	float d_u_cell_float = 0.0f;
	float d_v_cell_float = 0.0f;
	float d_w_cell_float = 0.0f;
	
	bool inOrigin = false;
	int seg_id = 0;
	for (auto segment_ids_it = p_segmentIds.begin(); segment_ids_it != p_segmentIds.end(); segment_ids_it++)
	{
		if (m_structureGraph->m_nodes->find(*segment_ids_it) == m_structureGraph->m_nodes->end()) continue;   //skip this segments. It's an edge!!
		p_node = m_structureGraph->m_nodes->at(*segment_ids_it)->segment;
		seg_id = p_node->id;
		std::vector<int> * seedPoints = (*p_downsampledSegmnets)[seg_id];
		if (seg_id == p_node_id) { inOrigin=true; } else inOrigin=false;		
		for (auto s_points_it = seedPoints->begin(); s_points_it != seedPoints->end(); s_points_it++)		
		{
			p = m_pointCloud->m_pointList.at(*s_points_it);
			p_translated = p.m_vertex.Position - o_m_xyz;
			Matrix3x3::Product(yz_rotation_i, p_translated, p_uvw_r);			

			d_u_cell_float = (p_uvw_r.Dot(uvwU) - mm_uvw.X) * g_c_s_u;			

			if (d_u_cell_float < -1) { points_outside_grid_u_neg++; d_u_cell = (int)d_u_cell_float-1; if (d_u_cell < dist_outside_grid_u_neg) dist_outside_grid_u_neg = d_u_cell; out=true; }
			else
			{
				d_u_cell = (int) d_u_cell_float;
				if (d_u_cell_float < 0) d_u_cell = 0;
				if (d_u_cell > u_c_c) { points_outside_grid_u_pos++; if (d_u_cell > dist_outside_grid_u_pos) dist_outside_grid_u_pos = d_u_cell; out = true; }
			}

			d_v_cell_float = (p_uvw_r.Dot(uvwV) - mm_uvw.Y) * g_c_s_v;
			if (d_v_cell_float < 0) { points_outside_grid_v_neg++; d_v_cell = (int)d_v_cell_float-1; if (d_v_cell < dist_outside_grid_v_neg) dist_outside_grid_v_neg = d_v_cell; out=true; } 
			else
			{
				d_v_cell = (int) d_v_cell_float;
				if (d_v_cell > v_c_c) { points_outside_grid_v_pos++; if (d_v_cell > dist_outside_grid_v_pos) dist_outside_grid_v_pos = d_v_cell; out = true; }
			}

			d_w_cell_float = (p_uvw_r.Dot(uvwW) - mm_uvw.Z) * g_c_s_w;
			if (d_w_cell_float < 0) { points_outside_grid_w_neg++; d_w_cell = (int)d_w_cell_float-1; if (d_w_cell < dist_outside_grid_w_neg) dist_outside_grid_w_neg = d_w_cell; out=true; } 
			else
			{
				d_w_cell = (int) d_w_cell_float;
				if (d_w_cell > w_c_c) { points_outside_grid_w_pos++; if (d_w_cell > dist_outside_grid_w_pos) dist_outside_grid_w_pos = d_w_cell; out = true; }
			}		
			
			if (!out) 
			{
				if (d_u_cell == previous_d_u_cell)
					if (d_v_cell == previous_d_v_cell)
						if (d_w_cell == previous_d_w_cell)
							_vgrid->IncActiveVoxelSurfaceCount(inOrigin);
						else
						{
							_vgrid->IncVoxelSurfaceCount(d_u_cell, d_v_cell, d_w_cell, inOrigin, seg_id);
							previous_d_u_cell = d_u_cell;
							previous_d_v_cell = d_v_cell;
							previous_d_w_cell = d_w_cell;
						}
					else
					{
						_vgrid->IncVoxelSurfaceCount(d_u_cell, d_v_cell, d_w_cell, inOrigin, seg_id);
						previous_d_u_cell = d_u_cell;
						previous_d_v_cell = d_v_cell;
						previous_d_w_cell = d_w_cell;
					}
				else
				{
					_vgrid->IncVoxelSurfaceCount(d_u_cell, d_v_cell, d_w_cell, inOrigin, seg_id);
					previous_d_u_cell = d_u_cell;
					previous_d_v_cell = d_v_cell;
					previous_d_w_cell = d_w_cell;
				}
			}
			out=false;
		}	
	}

	_vgrid->below_u_min = points_outside_grid_u_neg;
	_vgrid->above_u_max = points_outside_grid_u_pos;
	_vgrid->below_v_min = points_outside_grid_v_neg;
	_vgrid->above_v_max = points_outside_grid_v_pos;
	_vgrid->below_w_min = points_outside_grid_w_neg;
	_vgrid->above_w_max = points_outside_grid_w_pos;

	_vgrid->dist_below_u_min = dist_outside_grid_u_neg;
	_vgrid->dist_above_u_max = dist_outside_grid_u_pos;
	_vgrid->dist_below_v_min = dist_outside_grid_v_neg;
	_vgrid->dist_above_v_max = dist_outside_grid_v_pos;
	_vgrid->dist_below_w_min = dist_outside_grid_w_neg;
	_vgrid->dist_above_w_max = dist_outside_grid_w_pos;
}

void PointCloudSegmentation::ComputeVoxelGridAroundSegment(std::vector<int> p_segmentIds, FeatureDescStructureGraph::VoxelGrid * p_vg, int p_gridLod, float _rotationVWPlane, 
																					Vector3 &min_uvw_translate, Vector3 &max_uvw_translate, FeatureDescStructureGraph::VoxelGrid * _vgrid)
{
	_vgrid->m_voxels->clear();
	_vgrid->u_cells_count = p_vg->u_cells_count/p_gridLod;
	_vgrid->v_cells_count = p_vg->v_cells_count/p_gridLod;
	_vgrid->w_cells_count = p_vg->w_cells_count/p_gridLod;
	
	_vgrid->o_grid_u = p_vg->o_grid_u/p_gridLod; 
	_vgrid->o_grid_v = p_vg->o_grid_v/p_gridLod;	
	_vgrid->o_grid_w = p_vg->o_grid_w/p_gridLod;
				
	int p_segmentId = p_segmentIds.front();
	StructureGraph::Node * node = m_structureGraph->m_nodes->at(p_segmentId);
	SegmentsGraph::SurfaceSegment * p_node = node->segment;
	
	_vgrid->o_model_xyz = (p_node->obb.p_0_min.Distance + ((p_node->obb.p_0_max.Distance - p_node->obb.p_0_min.Distance) /2)) * (p_node->obb.obb_0) +
						(p_node->obb.p_1_min.Distance + ((p_node->obb.p_1_max.Distance - p_node->obb.p_1_min.Distance) /2)) * (p_node->obb.obb_1) +
						(p_node->obb.p_2_min.Distance + ((p_node->obb.p_2_max.Distance - p_node->obb.p_2_min.Distance) /2)) * (p_node->obb.obb_2);

	_vgrid->uvw.U = p_node->obb.obb_0;

	Matrix3x3 yz_rotation = Matrix3x3::CreateRotation(_vgrid->uvw.U, _rotationVWPlane);
	_vgrid->rotation = _rotationVWPlane;

	_vgrid->uvw.V = p_node->obb.obb_1;
	_vgrid->uvw.W = p_node->obb.obb_2;
	
	float min_u = 100000.f;
	float min_v = 100000.f;
	float min_w = 100000.f;

	float max_u = -100000.f;
	float max_v = -100000.f;
	float max_w = -100000.f;

	float d_u; float d_v; float d_w;

	std::vector<Vector3> obb_corners = p_node->obb.GetOBBCorners();	
	Vector3 p_tr;
	for (auto corners : obb_corners)
	{
		p_tr = corners - _vgrid->o_model_xyz;		
		d_u = p_tr.Dot(_vgrid->uvw.U);
		d_v = p_tr.Dot(_vgrid->uvw.V);
		d_w = p_tr.Dot(_vgrid->uvw.W);
		if (d_u > max_u) max_u = d_u; if (d_u < min_u) min_u = d_u;
		if (d_v > max_v) max_v = d_v; if (d_v < min_v) min_v = d_v;
		if (d_w > max_w) max_w = d_w; if (d_w < min_w) min_w = d_w;
	}

	_vgrid->min_obb_uvw = Vector3(min_u, min_v, min_w) - min_uvw_translate;
	_vgrid->max_obb_uvw = Vector3(max_u, max_v, max_w) + max_uvw_translate;
	
	Vector3 obb_span_model_segment = p_vg->max_obb_uvw - p_vg->min_obb_uvw;
	Vector3 obb_span_scene_segment = _vgrid->max_obb_uvw - _vgrid->min_obb_uvw;

	//Scale (non uniform) is set as the ratio betwween the U,V,W spans of the scene obb and the model obb.
	float scale_factor_u = (obb_span_scene_segment.X / obb_span_model_segment.X) * p_gridLod;
	float scale_factor_v = (obb_span_scene_segment.Y / obb_span_model_segment.Y) * p_gridLod;
	float scale_factor_w = (obb_span_scene_segment.Z / obb_span_model_segment.Z) * p_gridLod;

	Vector3 scale = Vector3(scale_factor_u, scale_factor_v, scale_factor_w);

	//std::cout << "Scaling Factor (with LOD factor) :: " << scale_factor_u  << "," << scale_factor_v << "," << scale_factor_w << std::endl;

	_vgrid->grid_cell_spacing_u = p_vg->grid_cell_spacing_u * scale_factor_u;
	_vgrid->grid_cell_spacing_v = p_vg->grid_cell_spacing_v * scale_factor_v;
	_vgrid->grid_cell_spacing_w = p_vg->grid_cell_spacing_w * scale_factor_w;
	
	Vector3 min_m_xyz_ = p_vg->min_model_uvw * scale;
	Vector3 max_m_xyz_ = p_vg->max_model_uvw * scale;
	
	Vector3 min_m_xyz; Vector3 max_m_xyz;
	Matrix3x3 uvw_inverse = _vgrid->uvw.GetMatrix(); uvw_inverse.Transpose();					
						
	Matrix3x3::Product(uvw_inverse, min_m_xyz_, min_m_xyz);
	Matrix3x3::Product(uvw_inverse, max_m_xyz_, max_m_xyz);
	
	Vector3 min_model_xyz_rot;
	Vector3 max_model_xyz_rot;

	//std::cout << "Min Model XYZ: " << min_m_xyz.ToString() << " Max Model XYZ: " << max_m_xyz.ToString() << std::endl;	

	Vector3 mm_uvw; 
	mm_uvw.X = min_m_xyz.Dot(_vgrid->uvw.U);
	mm_uvw.Y = min_m_xyz.Dot(_vgrid->uvw.V);
	mm_uvw.Z = min_m_xyz.Dot(_vgrid->uvw.W);

	_vgrid->min_model_uvw_br.X = mm_uvw.X;
	_vgrid->min_model_uvw_br.Y = mm_uvw.Y;
	_vgrid->min_model_uvw_br.Z = mm_uvw.Z;
		
	Matrix3x3::Product(yz_rotation, min_m_xyz, min_model_xyz_rot);
	Matrix3x3::Product(yz_rotation, max_m_xyz, max_model_xyz_rot);

	_vgrid->min_model_uvw.X = min_model_xyz_rot.Dot(_vgrid->uvw.U);
	_vgrid->min_model_uvw.Y = min_model_xyz_rot.Dot(_vgrid->uvw.V);
	_vgrid->min_model_uvw.Z = min_model_xyz_rot.Dot(_vgrid->uvw.W);
	_vgrid->max_model_uvw.X = max_model_xyz_rot.Dot(_vgrid->uvw.U);
	_vgrid->max_model_uvw.Y = max_model_xyz_rot.Dot(_vgrid->uvw.V);
	_vgrid->max_model_uvw.Z = max_model_xyz_rot.Dot(_vgrid->uvw.W);
	//std::cout << "NEW_VG MinUVW::" << new_vg->min_model_uvw.ToString() << " MaxUVW::" << new_vg->max_model_uvw.ToString() << std::endl;
		
	int d_u_cell;
	int d_v_cell;
	int d_w_cell;

	int points_outside_grid_u_pos = 0;
	int points_outside_grid_u_neg = 0;
	int points_outside_grid_v_pos = 0;
	int points_outside_grid_v_neg = 0;
	int points_outside_grid_w_pos = 0;
	int points_outside_grid_w_neg = 0;

	int dist_outside_grid_u_pos = 0;
	int dist_outside_grid_u_neg = 0;
	int dist_outside_grid_v_pos = 0;
	int dist_outside_grid_v_neg = 0;
	int dist_outside_grid_w_pos = 0;
	int dist_outside_grid_w_neg = 0;

	Vector3 p_uvw;
	Vector3 p_translated;
	Vector3 p_uvw_r;	
	Matrix3x3 uvw_m = _vgrid->uvw.GetMatrix();
	
	Matrix3x3 yz_rotation_i = Matrix3x3::CreateRotation(_vgrid->uvw.U, -_rotationVWPlane);
	CloudPoint p = CloudPoint(0,0,0);
	bool out = false;

	Vector3 o_m_xyz = _vgrid->o_model_xyz;
	float g_c_s_u = 1/_vgrid->grid_cell_spacing_u;
	float g_c_s_v = 1/_vgrid->grid_cell_spacing_v;
	float g_c_s_w = 1/_vgrid->grid_cell_spacing_w;
	Vector3 uvwU = _vgrid->uvw.U;
	Vector3 uvwV = _vgrid->uvw.V;
	Vector3 uvwW = _vgrid->uvw.W;
	int u_c_c = _vgrid->u_cells_count;
	int v_c_c = _vgrid->v_cells_count;
	int w_c_c = _vgrid->w_cells_count;

	int previous_d_u_cell = -1;
	int previous_d_v_cell = -1;
	int previous_d_w_cell = -1;	

	float d_u_cell_float = 0.0f;
	float d_v_cell_float = 0.0f;
	float d_w_cell_float = 0.0f;
	
	for (auto segment_ids_it = p_segmentIds.begin(); segment_ids_it != p_segmentIds.end(); segment_ids_it++)
	{
		p_node = m_structureGraph->m_nodes->at(*segment_ids_it)->segment;
		for (auto s_points_it =  p_node->points->begin(); s_points_it != p_node->points->end(); s_points_it++)
		{
			p = m_pointCloud->m_pointList.at(*s_points_it);
			p_translated = p.m_vertex.Position - o_m_xyz;
			Matrix3x3::Product(yz_rotation_i, p_translated, p_uvw_r);

			d_u_cell_float = (p_uvw_r.Dot(uvwU) - mm_uvw.X) * g_c_s_u;
			if (d_u_cell_float < 0) { points_outside_grid_u_neg++; d_u_cell = (int)d_u_cell_float-1; if (d_u_cell < dist_outside_grid_u_neg) dist_outside_grid_u_neg = d_u_cell; out=true; } 
			else
			{
				d_u_cell = (int) d_u_cell_float;
				if (d_u_cell > u_c_c) { points_outside_grid_u_pos++; if (d_u_cell > dist_outside_grid_u_pos) dist_outside_grid_u_pos = d_u_cell; out = true; }
			}

			d_v_cell_float = (p_uvw_r.Dot(uvwV) - mm_uvw.Y) * g_c_s_v;
			if (d_v_cell_float < 0) { points_outside_grid_v_neg++; d_v_cell = (int)d_v_cell_float-1; if (d_v_cell < dist_outside_grid_v_neg) dist_outside_grid_v_neg = d_v_cell; out=true; } 
			else
			{
				d_v_cell = (int) d_v_cell_float;
				if (d_v_cell > v_c_c) { points_outside_grid_v_pos++; if (d_v_cell > dist_outside_grid_v_pos) dist_outside_grid_v_pos = d_v_cell; out = true; }
			}

			d_w_cell_float = (p_uvw_r.Dot(uvwW) - mm_uvw.Z) * g_c_s_w;
			if (d_w_cell_float < 0) { points_outside_grid_w_neg++; d_w_cell = (int)d_w_cell_float-1; if (d_w_cell < dist_outside_grid_w_neg) dist_outside_grid_w_neg = d_w_cell; out=true; } 
			else
			{
				d_w_cell = (int) d_w_cell_float;
				if (d_w_cell > w_c_c) { points_outside_grid_w_pos++; if (d_w_cell > dist_outside_grid_w_pos) dist_outside_grid_w_pos = d_w_cell; out = true; }
			}			
			
			if (!out) 
			{
				if (d_u_cell == previous_d_u_cell)
					if (d_v_cell == previous_d_v_cell)
						if (d_w_cell == previous_d_w_cell)
							_vgrid->IncActiveVoxelSurfaceCount(true);
						else
						{
							_vgrid->IncVoxelSurfaceCount(d_u_cell, d_v_cell, d_w_cell, true, p_node->id);
							previous_d_u_cell = d_u_cell;
							previous_d_v_cell = d_v_cell;
							previous_d_w_cell = d_w_cell;
						}
					else
					{
						_vgrid->IncVoxelSurfaceCount(d_u_cell, d_v_cell, d_w_cell, true, p_node->id);
						previous_d_u_cell = d_u_cell;
						previous_d_v_cell = d_v_cell;
						previous_d_w_cell = d_w_cell;
					}
				else
				{
					_vgrid->IncVoxelSurfaceCount(d_u_cell, d_v_cell, d_w_cell, true, p_node->id);
					previous_d_u_cell = d_u_cell;
					previous_d_v_cell = d_v_cell;
					previous_d_w_cell = d_w_cell;
				}
			}
			out=false;
		}	
	}		

	_vgrid->below_u_min = points_outside_grid_u_neg;
	_vgrid->above_u_max = points_outside_grid_u_pos;
	_vgrid->below_v_min = points_outside_grid_v_neg;
	_vgrid->above_v_max = points_outside_grid_v_pos;
	_vgrid->below_w_min = points_outside_grid_w_neg;
	_vgrid->above_w_max = points_outside_grid_w_pos;

	_vgrid->dist_below_u_min = dist_outside_grid_u_neg;
	_vgrid->dist_above_u_max = dist_outside_grid_u_pos;
	_vgrid->dist_below_v_min = dist_outside_grid_v_neg;
	_vgrid->dist_above_v_max = dist_outside_grid_v_pos;
	_vgrid->dist_below_w_min = dist_outside_grid_w_neg;
	_vgrid->dist_above_w_max = dist_outside_grid_w_pos;
	
}

FeatureDescStructureGraph::VoxelGrid * PointCloudSegmentation::ComputeVoxelGridAroundSegment(std::vector<int> p_segmentIds, FeatureDescStructureGraph::VoxelGrid * p_vg, int _gridLod, 
																							 float _rotationVWPlane, Vector3 &min_uvw_translate, Vector3 &max_uvw_translate)
{
	FeatureDescStructureGraph::VoxelGrid * new_vg = new FeatureDescStructureGraph::VoxelGrid(p_vg->u_cells_count/_gridLod, p_vg->v_cells_count/_gridLod, p_vg->w_cells_count/_gridLod);
	new_vg->o_grid_u = p_vg->o_grid_u/_gridLod; new_vg->o_grid_v = p_vg->o_grid_v/_gridLod;	new_vg->o_grid_w = p_vg->o_grid_w/_gridLod;
	
	int p_segmentId = p_segmentIds.front();
	StructureGraph::Node * node = m_structureGraph->m_nodes->at(p_segmentId);
	SegmentsGraph::SurfaceSegment * p_node = node->segment;
	
	new_vg->o_model_xyz = (p_node->obb.p_0_min.Distance + ((p_node->obb.p_0_max.Distance - p_node->obb.p_0_min.Distance) /2)) * (p_node->obb.obb_0) +
						(p_node->obb.p_1_min.Distance + ((p_node->obb.p_1_max.Distance - p_node->obb.p_1_min.Distance) /2)) * (p_node->obb.obb_1) +
						(p_node->obb.p_2_min.Distance + ((p_node->obb.p_2_max.Distance - p_node->obb.p_2_min.Distance) /2)) * (p_node->obb.obb_2);

	new_vg->uvw.U = p_node->obb.obb_0;	

	Matrix3x3 yz_rotation = Matrix3x3::CreateRotation(new_vg->uvw.U, _rotationVWPlane);
	new_vg->rotation = _rotationVWPlane;	

	new_vg->uvw.V = p_node->obb.obb_1; 
	new_vg->uvw.W = p_node->obb.obb_2;	
		
	float min_u = 100000.f;
	float min_v = 100000.f;
	float min_w = 100000.f;

	float max_u = -100000.f;
	float max_v = -100000.f;
	float max_w = -100000.f;

	float d_u; float d_v; float d_w;

	//std::cout << "Multiplying by OBB scale:: " << obb_scale.ToString() << std::endl;
	
	std::vector<Vector3> obb_corners = p_node->obb.GetOBBCorners();	
	Vector3 p_tr;
	for (auto corners : obb_corners)
	{
		p_tr = corners - new_vg->o_model_xyz;				
		d_u = p_tr.Dot(new_vg->uvw.U);
		d_v = p_tr.Dot(new_vg->uvw.V);
		d_w = p_tr.Dot(new_vg->uvw.W);
		if (d_u > max_u) max_u = d_u; if (d_u < min_u) min_u = d_u;
		if (d_v > max_v) max_v = d_v; if (d_v < min_v) min_v = d_v;
		if (d_w > max_w) max_w = d_w; if (d_w < min_w) min_w = d_w;
	}

	new_vg->min_obb_uvw = Vector3(min_u, min_v, min_w) - min_uvw_translate;
	new_vg->max_obb_uvw = Vector3(max_u, max_v, max_w) + max_uvw_translate;
	
	Vector3 obb_span_model_segment = p_vg->max_obb_uvw - p_vg->min_obb_uvw;
	Vector3 obb_span_scene_segment = new_vg->max_obb_uvw - new_vg->min_obb_uvw;

	//Scale (non uniform) is set as the ratio betwween the U,V,W spans of the scene obb and the model obb.
	float scale_factor_u = (obb_span_scene_segment.X / obb_span_model_segment.X) * _gridLod;
	float scale_factor_v = (obb_span_scene_segment.Y / obb_span_model_segment.Y) * _gridLod;
	float scale_factor_w = (obb_span_scene_segment.Z / obb_span_model_segment.Z) * _gridLod;

	Vector3 scale = Vector3(scale_factor_u, scale_factor_v, scale_factor_w);

	//std::cout << "Scaling Factor (with LOD factor) :: " << scale_factor_u  << "," << scale_factor_v << "," << scale_factor_w << std::endl;

	new_vg->grid_cell_spacing_u = p_vg->grid_cell_spacing_u * scale_factor_u;
	new_vg->grid_cell_spacing_v = p_vg->grid_cell_spacing_v * scale_factor_v;
	new_vg->grid_cell_spacing_w = p_vg->grid_cell_spacing_w * scale_factor_w;

	//std::cout << "New Grid VoxelCell Size = " << new_vg->grid_cell_spacing_u << "," << new_vg->grid_cell_spacing_v << "," << new_vg->grid_cell_spacing_w << std::endl;		
	//std::cout << "New Grid Origin Cell: " << new_vg->o_grid_u << "," << new_vg->o_grid_v << "," << new_vg->o_grid_w << std::endl;
	//std::cout << "New Grid COUNT_UVW: " << new_vg->u_cells_count << "," << new_vg->v_cells_count << "," << new_vg->w_cells_count << std::endl;
	//std::cout << "New Grid U:" <<  new_vg->uvw.U.ToString() << " V:" << new_vg->uvw.V.ToString() << " W:" << new_vg->uvw.W.ToString() << std::endl;
	
	//std::cout << "P_VG MinUVW::" << p_vg->min_model_uvw.ToString() << " MaxUVW::" << p_vg->max_model_uvw.ToString() << std::endl;
	Vector3 min_m_xyz_ = p_vg->min_model_uvw * scale;
	Vector3 max_m_xyz_ = p_vg->max_model_uvw * scale;
	
	Vector3 min_m_xyz; Vector3 max_m_xyz;
	Matrix3x3 uvw_inverse = new_vg->uvw.GetMatrix(); uvw_inverse.Transpose();					
						
	Matrix3x3::Product(uvw_inverse, min_m_xyz_, min_m_xyz);
	Matrix3x3::Product(uvw_inverse, max_m_xyz_, max_m_xyz);
	
	Vector3 min_model_xyz_rot;
	Vector3 max_model_xyz_rot;

	//std::cout << "Min Model XYZ: " << min_m_xyz.ToString() << " Max Model XYZ: " << max_m_xyz.ToString() << std::endl;	

	Vector3 mm_uvw; 
	mm_uvw.X = min_m_xyz.Dot(new_vg->uvw.U);
	mm_uvw.Y = min_m_xyz.Dot(new_vg->uvw.V);
	mm_uvw.Z = min_m_xyz.Dot(new_vg->uvw.W);

	new_vg->min_model_uvw_br.X = mm_uvw.X;
	new_vg->min_model_uvw_br.Y = mm_uvw.Y;
	new_vg->min_model_uvw_br.Z = mm_uvw.Z;		

	Matrix3x3::Product(yz_rotation, min_m_xyz, min_model_xyz_rot);
	Matrix3x3::Product(yz_rotation, max_m_xyz, max_model_xyz_rot);

	new_vg->min_model_uvw.X = min_model_xyz_rot.Dot(new_vg->uvw.U);
	new_vg->min_model_uvw.Y = min_model_xyz_rot.Dot(new_vg->uvw.V);
	new_vg->min_model_uvw.Z = min_model_xyz_rot.Dot(new_vg->uvw.W);
	new_vg->max_model_uvw.X = max_model_xyz_rot.Dot(new_vg->uvw.U);
	new_vg->max_model_uvw.Y = max_model_xyz_rot.Dot(new_vg->uvw.V);
	new_vg->max_model_uvw.Z = max_model_xyz_rot.Dot(new_vg->uvw.W);
	//std::cout << "NEW_VG MinUVW::" << new_vg->min_model_uvw.ToString() << " MaxUVW::" << new_vg->max_model_uvw.ToString() << std::endl;
		
	int d_u_cell;
	int d_v_cell;
	int d_w_cell;

	int points_outside_grid_u_pos = 0;
	int points_outside_grid_u_neg = 0;
	int points_outside_grid_v_pos = 0;
	int points_outside_grid_v_neg = 0;
	int points_outside_grid_w_pos = 0;
	int points_outside_grid_w_neg = 0;

	int dist_outside_grid_u_pos = 0;
	int dist_outside_grid_u_neg = 0;
	int dist_outside_grid_v_pos = 0;
	int dist_outside_grid_v_neg = 0;
	int dist_outside_grid_w_pos = 0;
	int dist_outside_grid_w_neg = 0;

	Vector3 p_uvw;
	Vector3 p_translated;
	Vector3 p_uvw_r;	
	Matrix3x3 uvw_m = new_vg->uvw.GetMatrix();
	
	Matrix3x3 yz_rotation_i = Matrix3x3::CreateRotation(new_vg->uvw.U, -_rotationVWPlane);
	CloudPoint p = CloudPoint(0,0,0);
	bool out = false;

	for (auto segment_ids_it = p_segmentIds.begin(); segment_ids_it != p_segmentIds.end(); segment_ids_it++)
	{
		p_node = m_structureGraph->m_nodes->at(*segment_ids_it)->segment;
		//std::cout << "Inserting in VG " << p_node->id << std::endl;
		for (auto s_points_it =  p_node->points->begin(); s_points_it != p_node->points->end(); s_points_it++)
		{
			p = m_pointCloud->m_pointList.at(*s_points_it);
			p_translated = p.m_vertex.Position - new_vg->o_model_xyz;
			Matrix3x3::Product(yz_rotation_i, p_translated, p_uvw_r);
		
			d_u_cell = Maths::Floor((p_uvw_r.Dot(new_vg->uvw.U) - mm_uvw.X) / new_vg->grid_cell_spacing_u);
			if (d_u_cell < 0) { points_outside_grid_u_neg++; if (d_u_cell < dist_outside_grid_u_neg) dist_outside_grid_u_neg = d_u_cell; out=true; }
			if (d_u_cell > new_vg->u_cells_count) { points_outside_grid_u_pos++; if (d_u_cell > dist_outside_grid_u_pos) dist_outside_grid_u_pos = d_u_cell; out = true; }
		
			d_v_cell = Maths::Floor((p_uvw_r.Dot(new_vg->uvw.V) - mm_uvw.Y) / new_vg->grid_cell_spacing_v);
			if (d_v_cell < 0) { points_outside_grid_v_neg++; out=true; if (d_v_cell < dist_outside_grid_v_neg) dist_outside_grid_v_neg = d_v_cell; }
			if (d_v_cell > new_vg->v_cells_count) { points_outside_grid_v_pos++; if (d_v_cell > dist_outside_grid_v_pos) dist_outside_grid_v_pos = d_v_cell; out=true; }

			d_w_cell = Maths::Floor((p_uvw_r.Dot(new_vg->uvw.W) - mm_uvw.Z) / new_vg->grid_cell_spacing_w);
			if (d_w_cell < 0) { points_outside_grid_w_neg++; if (d_w_cell < dist_outside_grid_w_neg) dist_outside_grid_w_neg = d_w_cell; out=true; }
			if (d_w_cell > new_vg->w_cells_count) { points_outside_grid_w_pos++; if (d_w_cell > dist_outside_grid_w_pos) dist_outside_grid_w_pos = d_w_cell; out=true; }

			if (!out) new_vg->IncVoxelSurfaceCount(d_u_cell, d_v_cell, d_w_cell, true, p_node->id);
			out=false;
		}	
	}		

	new_vg->below_u_min = points_outside_grid_u_neg;
	new_vg->above_u_max = points_outside_grid_u_pos;
	new_vg->below_v_min = points_outside_grid_v_neg;
	new_vg->above_v_max = points_outside_grid_v_pos;
	new_vg->below_w_min = points_outside_grid_w_neg;
	new_vg->above_w_max = points_outside_grid_w_pos;

	new_vg->dist_below_u_min = dist_outside_grid_u_neg;
	new_vg->dist_above_u_max = dist_outside_grid_u_pos;
	new_vg->dist_below_v_min = dist_outside_grid_v_neg;
	new_vg->dist_above_v_max = dist_outside_grid_v_pos;
	new_vg->dist_below_w_min = dist_outside_grid_w_neg;
	new_vg->dist_above_w_max = dist_outside_grid_w_pos;


	//std::cout << "Segment: " << p_node->id << ", UVW:" << new_vg->uvw.ToString() << " O: " << new_vg->o_model_xyz.ToString() << std::endl;
	//std::cout << "Grid Cell Count: U" << new_vg->u_cells_count << " V:" << new_vg->v_cells_count << " W:" << new_vg->w_cells_count << " Grid Origin: " << new_vg->o_grid_u <<","<< new_vg->o_grid_v <<","<<new_vg->o_grid_w << std::endl;
	//std::cout << "Along U: " << min_u << " <-> " << max_u << " | Along V: " << min_v << " <-> " << max_v << " | Along W: " << min_w << " <-> " << max_w << std::endl;
	//std::cout << "Model Origin(Centre) XYZ:: " << new_vg->o_model_xyz.ToString() << std::endl;
	//std::cout << "Model Origin(Centre) UVW:: " << Vector3(new_vg->o_model_u, new_vg->o_model_v, new_vg->o_model_w).ToString() << std::endl;
	//std::cout << "Grid Origin UVW:: " << Vector3(new_vg->o_grid_u, new_vg->o_grid_v, new_vg->o_grid_w).ToString() << std::endl;
	//std::cout << "Dropped points = " << points_outside_grid << " Number of Cells = " << new_vg->m_voxels->size() << std::endl;
	
	return new_vg;
}

//_segmentId refers to a surface segment found in _otherObject
std::pair<float, std::vector<int>> PointCloudSegmentation::ScoreFeatureMatrix(PointCloudSegmentation * _otherObject, int _segmentId) 
{	
	std::pair<float, std::vector<int>> scores;
	//1: Compute graph connectivity score.


	//2: Compute voxel grid intersection score.



	//3: Combine score and 

	return scores;
}

FeatureDescStructureGraph * PointCloudSegmentation::GetFDG(int _id)
{
	for (int i=0; i<m_featureDescriptionGraphs->size(); i++)
	{
		if (m_featureDescriptionGraphs->at(i)->initial_node_id == _id) 
			return m_featureDescriptionGraphs->at(i);
	}
	return NULL;
}

std::pair<Vector3, std::pair<float,float>> PointCloudSegmentation::ComputeRatioOfPointsAboveAndUnder(int p_segmentId, int p_iSegmentId)
{	
	std::pair<Vector3, std::pair<float,float>> ratio = std::pair<Vector3, std::pair<float,float>>(Vector3(0), std::pair<float,float>(0,0));
	auto segment_it = m_segmentsGraph->m_surfaceSegments->find(p_segmentId);
	if (segment_it==m_segmentsGraph->m_surfaceSegments->end()) return ratio;
	auto segment = segment_it->second;
	SegmentsGraph::SurfaceSegment * isegment;
	Vector3 over; Vector3 under;
	Plane plane;
	int over_count = 0; int under_count = 0;
	if (p_iSegmentId != 0) //get the internal segment
	{
		auto i_segment_it = segment->internalSegGraph->m_surfaceSegments->find(p_iSegmentId);
		if (i_segment_it==segment->internalSegGraph->m_surfaceSegments->end()) return ratio;
		isegment = i_segment_it->second;
		over = isegment->obb.obb_0;
		under = -over;		
		plane = isegment->obb.p_0_min;
		plane.Distance = isegment->obb.p_0_max.Distance; // + ((isegment->obb.p_0_max.Distance - isegment->obb.p_0_min.Distance)/2);
		ratio.first = isegment->obb.obb_0;
	}
	else
	{
		//SegmentsGraph::OBB ob = m_segmentsGraph->ComputeOBB(p_segmentId, 0.f);    //OBB should already have been computed.
		SegmentsGraph::OBB ob = segment->obb;
		over = ob.obb_0;
		under = -over;
		plane = ob.p_0_min;
		plane.Distance = ob.p_0_min.Distance + ((ob.p_0_max.Distance - ob.p_0_min.Distance)/2);
		ratio.first = ob.obb_0;

		/*over = segment->obb.obb_0;
		under = -over;
		plane = segment->obb.p_0_min;
		plane.Distance = segment->obb.p_0_min.Distance + ((segment->obb.p_0_max.Distance - segment->obb.p_0_min.Distance)/2);
		ratio.first = segment->obb.obb_0;*/
	}

	Vector3 obb_corners[8];	

	//Iterate over all surface segments in the structure graph.
	for (auto nodes_it = m_structureGraph->m_nodes->begin(); nodes_it != m_structureGraph->m_nodes->end(); nodes_it++)
	{
		if (nodes_it->first == p_segmentId) continue; //skip
		if (m_structureGraph->IsNodeMerged(nodes_it->first)) continue; //skip		
		auto c_segment = nodes_it->second->segment;
		auto c_segment_obb = c_segment->obb;

		float dot = Vector3::Dot(c_segment->obb.obb_0, segment->obb.obb_0);
		if (Maths::Abs(dot) > 0.95f)
		{
			float d1 = c_segment_obb.p_0_min.Distance + ((c_segment_obb.p_0_max.Distance - c_segment_obb.p_0_min.Distance) / 2);
			float d2 = segment->obb.p_0_min.Distance + ((segment->obb.p_0_max.Distance - segment->obb.p_0_min.Distance) / 2);
			float diff = (d1 * Maths::ISgn(dot)) - d2;
			if (Maths::Abs(diff) < 1.3f) continue; // Too Close .. do not consider in computation. Planes are probably coplanar (bar noise) if difference in distance of less than 1.3f
		}

		//First the eight points composing the obb of c_segment.
		obb_corners[0].Set(c_segment_obb.obb_0.X*c_segment_obb.p_0_min.Distance + c_segment_obb.obb_1.X*c_segment_obb.p_1_min.Distance + c_segment_obb.obb_2.X*c_segment_obb.p_2_min.Distance, 
							c_segment_obb.obb_0.Y*c_segment_obb.p_0_min.Distance + c_segment_obb.obb_1.Y*c_segment_obb.p_1_min.Distance + c_segment_obb.obb_2.Y*c_segment_obb.p_2_min.Distance, 
							c_segment_obb.obb_0.Z*c_segment_obb.p_0_min.Distance + c_segment_obb.obb_1.Z*c_segment_obb.p_1_min.Distance + c_segment_obb.obb_2.Z*c_segment_obb.p_2_min.Distance);  //0

		obb_corners[1].Set(c_segment_obb.obb_0.X*c_segment_obb.p_0_min.Distance + c_segment_obb.obb_1.X*c_segment_obb.p_1_max.Distance + c_segment_obb.obb_2.X*c_segment_obb.p_2_min.Distance, 
							c_segment_obb.obb_0.Y*c_segment_obb.p_0_min.Distance + c_segment_obb.obb_1.Y*c_segment_obb.p_1_max.Distance + c_segment_obb.obb_2.Y*c_segment_obb.p_2_min.Distance, 
							c_segment_obb.obb_0.Z*c_segment_obb.p_0_min.Distance + c_segment_obb.obb_1.Z*c_segment_obb.p_1_max.Distance + c_segment_obb.obb_2.Z*c_segment_obb.p_2_min.Distance);  //1

		obb_corners[2].Set(c_segment_obb.obb_0.X*c_segment_obb.p_0_min.Distance + c_segment_obb.obb_1.X*c_segment_obb.p_1_max.Distance + c_segment_obb.obb_2.X*c_segment_obb.p_2_max.Distance, 
							c_segment_obb.obb_0.Y*c_segment_obb.p_0_min.Distance + c_segment_obb.obb_1.Y*c_segment_obb.p_1_max.Distance + c_segment_obb.obb_2.Y*c_segment_obb.p_2_max.Distance, 
							c_segment_obb.obb_0.Z*c_segment_obb.p_0_min.Distance + c_segment_obb.obb_1.Z*c_segment_obb.p_1_max.Distance + c_segment_obb.obb_2.Z*c_segment_obb.p_2_max.Distance);  //2 

		obb_corners[3].Set(c_segment_obb.obb_0.X*c_segment_obb.p_0_min.Distance + c_segment_obb.obb_1.X*c_segment_obb.p_1_min.Distance + c_segment_obb.obb_2.X*c_segment_obb.p_2_max.Distance, 
							c_segment_obb.obb_0.Y*c_segment_obb.p_0_min.Distance + c_segment_obb.obb_1.Y*c_segment_obb.p_1_min.Distance + c_segment_obb.obb_2.Y*c_segment_obb.p_2_max.Distance, 
							c_segment_obb.obb_0.Z*c_segment_obb.p_0_min.Distance + c_segment_obb.obb_1.Z*c_segment_obb.p_1_min.Distance + c_segment_obb.obb_2.Z*c_segment_obb.p_2_max.Distance);  //3

		obb_corners[4].Set(c_segment_obb.obb_0.X*c_segment_obb.p_0_max.Distance + c_segment_obb.obb_1.X*c_segment_obb.p_1_min.Distance + c_segment_obb.obb_2.X*c_segment_obb.p_2_min.Distance, 
							c_segment_obb.obb_0.Y*c_segment_obb.p_0_max.Distance + c_segment_obb.obb_1.Y*c_segment_obb.p_1_min.Distance + c_segment_obb.obb_2.Y*c_segment_obb.p_2_min.Distance, 
							c_segment_obb.obb_0.Z*c_segment_obb.p_0_max.Distance + c_segment_obb.obb_1.Z*c_segment_obb.p_1_min.Distance + c_segment_obb.obb_2.Z*c_segment_obb.p_2_min.Distance);  //4

		obb_corners[5].Set(c_segment_obb.obb_0.X*c_segment_obb.p_0_max.Distance + c_segment_obb.obb_1.X*c_segment_obb.p_1_max.Distance + c_segment_obb.obb_2.X*c_segment_obb.p_2_min.Distance, 
							c_segment_obb.obb_0.Y*c_segment_obb.p_0_max.Distance + c_segment_obb.obb_1.Y*c_segment_obb.p_1_max.Distance + c_segment_obb.obb_2.Y*c_segment_obb.p_2_min.Distance, 
							c_segment_obb.obb_0.Z*c_segment_obb.p_0_max.Distance + c_segment_obb.obb_1.Z*c_segment_obb.p_1_max.Distance + c_segment_obb.obb_2.Z*c_segment_obb.p_2_min.Distance);  //5

		obb_corners[6].Set(c_segment_obb.obb_0.X*c_segment_obb.p_0_max.Distance + c_segment_obb.obb_1.X*c_segment_obb.p_1_max.Distance + c_segment_obb.obb_2.X*c_segment_obb.p_2_max.Distance, 
							c_segment_obb.obb_0.Y*c_segment_obb.p_0_max.Distance + c_segment_obb.obb_1.Y*c_segment_obb.p_1_max.Distance + c_segment_obb.obb_2.Y*c_segment_obb.p_2_max.Distance, 
							c_segment_obb.obb_0.Z*c_segment_obb.p_0_max.Distance + c_segment_obb.obb_1.Z*c_segment_obb.p_1_max.Distance + c_segment_obb.obb_2.Z*c_segment_obb.p_2_max.Distance);  //6

		obb_corners[7].Set(c_segment_obb.obb_0.X*c_segment_obb.p_0_max.Distance + c_segment_obb.obb_1.X*c_segment_obb.p_1_min.Distance + c_segment_obb.obb_2.X*c_segment_obb.p_2_max.Distance, 
							c_segment_obb.obb_0.Y*c_segment_obb.p_0_max.Distance + c_segment_obb.obb_1.Y*c_segment_obb.p_1_min.Distance + c_segment_obb.obb_2.Y*c_segment_obb.p_2_max.Distance, 
							c_segment_obb.obb_0.Z*c_segment_obb.p_0_max.Distance + c_segment_obb.obb_1.Z*c_segment_obb.p_1_min.Distance + c_segment_obb.obb_2.Z*c_segment_obb.p_2_max.Distance);  //7

		//Check 1. All obb points above or below
		int pos_count = 0;
		int neg_count = 0;
		int over_count_t = 0;
		int under_count_t = 0;

		for (int i=0; i<8; i++)
		{
			if (plane.GetSide(obb_corners[i]) == Plane::Side_Positive) pos_count++; else neg_count++;
		}

		if (pos_count == 8 || neg_count == 8)
		{
			//Fine OBB lies entirely on one side of the segment
			if (pos_count == 8)
			{
				over_count_t += c_segment->points->size();
			}
			else
			{
				under_count_t += c_segment->points->size();
			}
		}
		else
		{
			//Need to check for every point in the segment
			//First Check if this node has other nodes merged into it.
			auto mergedNodes = m_structureGraph->m_mergedNodesSeeds->find(nodes_it->first);
			if (mergedNodes != m_structureGraph->m_mergedNodesSeeds->end())
			{
				//First go through the seed segment
				for (auto points_it = c_segment->points->begin(); points_it != c_segment->points->end(); points_it++)
				{
					if (plane.GetSide(m_pointCloud->m_pointList[*points_it].m_vertex.Position) == Plane::Side_Positive) over_count_t++; else under_count_t++;
				}
				//Then go through all points in the merged segments.
				for (int i = 0; i<mergedNodes->second.size(); i++)
				{					
					auto m_segment = m_structureGraph->m_segmentsGraph->m_surfaceSegments->at(mergedNodes->second[i]);
					for (auto points_it = m_segment->points->begin(); points_it != m_segment->points->end(); points_it++)
					{
						if (plane.GetSide(m_pointCloud->m_pointList[*points_it].m_vertex.Position) == Plane::Side_Positive) over_count_t++; else under_count_t++;
					}
				}
			}
			else
			{
				for (auto points_it = c_segment->points->begin(); points_it != c_segment->points->end(); points_it++)
				{
					if (plane.GetSide(m_pointCloud->m_pointList[*points_it].m_vertex.Position) == Plane::Side_Positive) over_count_t++; else under_count_t++;
				}
			}
		}

		over_count += over_count_t;
		under_count += under_count_t;
	}

	//std::cout << "Over_Under_Ratio:: Segment " << p_segmentId << "." << p_iSegmentId << " == Over(" << over_count << ") | Under(" << under_count << ")" << std::endl;

	if (under_count == 0)
	{
		ratio.second.first = 1.f;
		ratio.second.second = 0.f;
	}

	if (over_count == 0)
	{
		ratio.second.first = 0.f;
		ratio.second.second = 1.f;
	}

	if (under_count > 0 && over_count > 0)
	{
		ratio.second.first = (float)over_count / ((float)over_count + (float)under_count);
		ratio.second.second = (float)under_count / ((float)over_count + (float)under_count);
	}	

	return ratio;
}

void PointCloudSegmentation::PrintOptimalFeatureStrings()
{
	std::stringstream mess;

	for (int i=0; i<m_featureDescriptionGraphs->size(); i++)
	{
		std::pair<float, std::vector<int>> maxCoverageStr_Length2 = m_featureDescriptionGraphs->at(i)->GetStringDescriptorMaxCoverage(2);
		std::pair<float, std::vector<int>> maxCoverageStr_Length3 = m_featureDescriptionGraphs->at(i)->GetStringDescriptorMaxCoverage(3);
		std::pair<float, std::vector<int>> maxCoverageStr_Length4 = m_featureDescriptionGraphs->at(i)->GetStringDescriptorMaxCoverage(4);
		std::pair<float, std::vector<int>> maxCoverageStr_Length5 = m_featureDescriptionGraphs->at(i)->GetStringDescriptorMaxCoverage(5);

		std::pair<float, std::vector<int>> maxPointCountStr_Length2 = m_featureDescriptionGraphs->at(i)->GetStringDescriptorMaxPointCount(2);
		std::pair<float, std::vector<int>> maxPointCountStr_Length3 = m_featureDescriptionGraphs->at(i)->GetStringDescriptorMaxPointCount(3);
		std::pair<float, std::vector<int>> maxPointCountStr_Length4 = m_featureDescriptionGraphs->at(i)->GetStringDescriptorMaxPointCount(4);
		std::pair<float, std::vector<int>> maxPointCountStr_Length5 = m_featureDescriptionGraphs->at(i)->GetStringDescriptorMaxPointCount(5);

		std::pair<float, std::vector<int>> maxPointCountCrossCoverageStr_Length2 = m_featureDescriptionGraphs->at(i)->GetStringDescriptorMaxCoverageCrossPointCount(2);
		std::pair<float, std::vector<int>> maxPointCountCrossCoverageStr_Length3 = m_featureDescriptionGraphs->at(i)->GetStringDescriptorMaxCoverageCrossPointCount(3);
		std::pair<float, std::vector<int>> maxPointCountCrossCoverageStr_Length4 = m_featureDescriptionGraphs->at(i)->GetStringDescriptorMaxCoverageCrossPointCount(4);
		std::pair<float, std::vector<int>> maxPointCountCrossCoverageStr_Length5 = m_featureDescriptionGraphs->at(i)->GetStringDescriptorMaxCoverageCrossPointCount(5);

		mess << "[Compute Feature Structures] MaxCoverageString Length2. Coverage = " << maxCoverageStr_Length2.first << " String =";
		for (std::vector<int>::iterator it = maxCoverageStr_Length2.second.begin(); it != maxCoverageStr_Length2.second.end(); it++) mess << " " << *it;
		mess << std::endl;

		mess << "[Compute Feature Structures] MaxCoverageString Length3. Coverage = " << maxCoverageStr_Length3.first << " String =";
		for (std::vector<int>::iterator it = maxCoverageStr_Length3.second.begin(); it != maxCoverageStr_Length3.second.end(); it++) mess << " " << *it;
		mess << std::endl;

		mess << "[Compute Feature Structures] MaxCoverageString Length4. Coverage = " << maxCoverageStr_Length4.first << " String =";
		for (std::vector<int>::iterator it = maxCoverageStr_Length4.second.begin(); it != maxCoverageStr_Length4.second.end(); it++) mess << " " << *it;
		mess << std::endl;

		mess << "[Compute Feature Structures] MaxCoverageString Length5. Coverage = " << maxCoverageStr_Length5.first << " String =";
		for (std::vector<int>::iterator it = maxCoverageStr_Length5.second.begin(); it != maxCoverageStr_Length5.second.end(); it++) mess << " " << *it;
		mess << std::endl;

		mess << "[Compute Feature Structures] MaxPointCountString Length2. PointCount = " << maxPointCountStr_Length2.first << " String =";
		for (std::vector<int>::iterator it = maxPointCountStr_Length2.second.begin(); it != maxPointCountStr_Length2.second.end(); it++) mess << " " << *it;
		mess << std::endl;

		mess << "[Compute Feature Structures] MaxPointCountString Length3. PointCount = " << maxPointCountStr_Length3.first << " String =";
		for (std::vector<int>::iterator it = maxPointCountStr_Length3.second.begin(); it != maxPointCountStr_Length3.second.end(); it++) mess << " " << *it;
		mess << std::endl;

		mess << "[Compute Feature Structures] MaxPointCountString Length4. PointCount = " << maxPointCountStr_Length4.first << " String =";
		for (std::vector<int>::iterator it = maxPointCountStr_Length4.second.begin(); it != maxPointCountStr_Length4.second.end(); it++) mess << " " << *it;
		mess << std::endl;

		mess << "[Compute Feature Structures] MaxPointCountString Length5. PointCount = " << maxPointCountStr_Length5.first << " String =";
		for (std::vector<int>::iterator it = maxPointCountStr_Length5.second.begin(); it != maxPointCountStr_Length5.second.end(); it++) mess << " " << *it;
		mess << std::endl;

		mess << "[Compute Feature Structures] MaxPointCountCrossCoverageString Length2. PointCount = " << maxPointCountCrossCoverageStr_Length2.first << " String =";
		for (std::vector<int>::iterator it = maxPointCountCrossCoverageStr_Length2.second.begin(); it != maxPointCountCrossCoverageStr_Length2.second.end(); it++) mess << " " << *it;
		mess << std::endl;

		mess << "[Compute Feature Structures] MaxPointCountCrossCoverageString Length3. PointCount = " << maxPointCountCrossCoverageStr_Length3.first << " String =";
		for (std::vector<int>::iterator it = maxPointCountCrossCoverageStr_Length3.second.begin(); it != maxPointCountCrossCoverageStr_Length3.second.end(); it++) mess << " " << *it;
		mess << std::endl;

		mess << "[Compute Feature Structures] MaxPointCountCrossCoverageString Length4. PointCount = " << maxPointCountCrossCoverageStr_Length4.first << " String =";
		for (std::vector<int>::iterator it = maxPointCountCrossCoverageStr_Length4.second.begin(); it != maxPointCountCrossCoverageStr_Length4.second.end(); it++) mess << " " << *it;
		mess << std::endl;

		mess << "[Compute Feature Structures] MaxPointCountCrossCoverageString Length5. PointCount = " << maxPointCountCrossCoverageStr_Length5.first << " String =";
		for (std::vector<int>::iterator it = maxPointCountCrossCoverageStr_Length5.second.begin(); it != maxPointCountCrossCoverageStr_Length5.second.end(); it++) mess << " " << *it;
		mess << std::endl;
		mess << std::endl;
		mess << std::endl;

		(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Critical);
		mess.str(std::string(""));
	}
}

//After the FULL segments, edge and structure graphs have been built use individual views on the object to add/refine info (e.g. hidden edges) to the graphs	
//This function is only called on the FULL model PCS instance.
//We first need to perform some alignment process in order to make sure the partial and full pointclouds are mapped correctly. Just using Offset is "wrong" as some points might have been removed (e.g. duplicates)
void PointCloudSegmentation::EnhanceGraphsFromSingleView(PointCloudSegmentation* p_viewPCS, int p_viewIdx)
{	
	std::stringstream mess; mess << "\n[Enhancing Graph from View] " << m_pointCloud->GetName() << " using " << p_viewPCS->m_pointCloud->GetName();
	(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Critical);
	int offset = 0;
	int match = 0;
	std::map<int, SegmentsGraph::SurfaceSegment*>::iterator viewSurfaceSegmentsIterator;
	std::map<std::pair<int,int>, int> segmentsMap;   //Key->first = segmentid in full pointcloud, key->second = segment id in view pointcloud. Value = Number of common points.	
	int view_segmentId;
	int full_idx;
	int view_idx;
	std::pair<int,int> key;

	std::map<int,int> activePointsMap;
	Vector3 firstPointInView = p_viewPCS->m_pointCloud->m_pointList[0].m_vertex.Position;
	for (int i = 0; i<p_viewPCS->m_pointCloud->m_pointList.size(); i++)
	{
		for (int j = offset; j<m_pointCloud->m_pointList.size(); j++) 
		{
			if (p_viewPCS->m_pointCloud->m_pointList[i].m_vertex.Position.Equals(m_pointCloud->m_pointList[j].m_vertex.Position)) 
			{ 
				offset = j; 
				activePointsMap[i] = j;
				break; 
			}
		}
	}			
	
	mess.str(std::string("")); mess << "[Enhancing Graph from View] Segments alignment map created. Matched " << activePointsMap.size() << " out of " << p_viewPCS->m_pointCloud->m_pointList.size();
	(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Critical);
	std::map<int,int>::iterator activePointsIterator;

	std::map<std::pair<int,int>, int>::iterator segmentsMapIt;
	CloudPoint full_cp(0,0,0);
	CloudPoint view_cp(0,0,0);

	for (viewSurfaceSegmentsIterator = p_viewPCS->m_segmentsGraph->m_surfaceSegments->begin(); 
			viewSurfaceSegmentsIterator != p_viewPCS->m_segmentsGraph->m_surfaceSegments->end(); viewSurfaceSegmentsIterator++)
	{		
		view_segmentId = viewSurfaceSegmentsIterator->second->id;
		int view_segment_size = viewSurfaceSegmentsIterator->second->points->size();
		key.second = view_segmentId;		
		for (int i = 0; i<view_segment_size; i++)
		{
			view_idx = viewSurfaceSegmentsIterator->second->points->at(i);
			activePointsIterator = activePointsMap.find(view_idx);
			if (activePointsIterator == activePointsMap.end()) continue;
			full_idx = activePointsIterator->second;
			full_cp = m_pointCloud->m_pointList[full_idx];
			if (full_cp.m_typeId!=4) continue;

			view_cp = p_viewPCS->m_pointCloud->m_pointList[view_idx];			
			if (i % 400 == 0) 
			{
				mess.str(std::string("")); mess << "ViewPos[" << view_idx << "]=" << view_cp.m_vertex.Position.ToString() << "  " << "FullPos[" << full_idx << "]=" << full_cp.m_vertex.Position.ToString();
				(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Info);					
			}
			key.first = full_cp.m_segmentId;
			segmentsMapIt = segmentsMap.find(key);
			if (segmentsMapIt == segmentsMap.end()) 
			{
				segmentsMap[key] = 1;
			}
			else
			{
				segmentsMapIt->second += 1;
			}
		}
	}

	mess.str(std::string("")); mess << "[Enhancing Graph from View] Point Matching for View Created. Matched " << segmentsMap.size();
	(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Critical);
	
	//Print Surface-Surface Mapping
	//Update visibility metric scores for all surface states what are visible from this view.
	std::map<int, int> visibleSurfacesDecompositionCount;
	for (std::map<std::pair<int,int>, int>::iterator keyIterator = segmentsMap.begin(); keyIterator != segmentsMap.end(); keyIterator++)
	{
		if (visibleSurfacesDecompositionCount.find(keyIterator->first.first) == visibleSurfacesDecompositionCount.end()) 
		{
			visibleSurfacesDecompositionCount[keyIterator->first.first] = 0;
			if (m_surfaceVisiblityCount.find(keyIterator->first.first) == m_surfaceVisiblityCount.end()) m_surfaceVisiblityCount[keyIterator->first.first] = 0;
			m_surfaceVisiblityCount[keyIterator->first.first]++;
		}

		visibleSurfacesDecompositionCount[keyIterator->first.first]++;
		
		int pntCntinFull = m_segmentsGraph->m_surfaceSegments->find(keyIterator->first.first)->second->points->size();
		int pntCntinView = p_viewPCS->m_segmentsGraph->m_surfaceSegments->find(keyIterator->first.second)->second->points->size();

		mess.str(std::string(""));
		mess << "Key=" << keyIterator->first.first << "[" << pntCntinFull << "]:" << keyIterator->first.second << "[" << pntCntinView << "] Intersection Points = " << keyIterator->second;
		(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Error);
	}

	//Update max surfaces viewed
	if (m_maxSurfacesVisibleFromViews < visibleSurfacesDecompositionCount.size()) 
	{
		m_maxSurfacesVisibleFromViews = visibleSurfacesDecompositionCount.size();

		mess.str(std::string("")); mess << "[Enhancing Graph from View] Setting Max Surfaces Visible :: " << m_maxSurfacesVisibleFromViews;
		(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Critical);			
	}
	
	//Establish occlusion data on edges in full view PCS - visibleSurfacesDecompositionCount stores the surfaces which are visible from this view.
	std::map<int, SegmentsGraph::EdgeSegment*> * fullViewEdgesConnected;
	std::map<int, SegmentsGraph::EdgeSegment*> * partialViewEdgesConnected;
	for (std::map<int, int>::iterator fullViewSurfaceSegmentsIterator = visibleSurfacesDecompositionCount.begin(); fullViewSurfaceSegmentsIterator != visibleSurfacesDecompositionCount.end();
		fullViewSurfaceSegmentsIterator++)
	{
		mess.str(std::string("")); mess << "******Checking visibility for edges connected to segment with ID " << fullViewSurfaceSegmentsIterator->first;
		(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Error);

		//Get the edges connected to this surface in full view
		fullViewEdgesConnected = m_segmentsGraph->SurfaceSegmentWithId(fullViewSurfaceSegmentsIterator->first)->connections;
		//Iterate over these edges and check if there are correspondences in the current partial view. 
		for (std::map<int, SegmentsGraph::EdgeSegment*>::iterator fullViewEdgeSegmentsIterator = fullViewEdgesConnected->begin(); 
			fullViewEdgeSegmentsIterator != fullViewEdgesConnected->end(); fullViewEdgeSegmentsIterator++)
		{
			SegmentsGraph::EdgeSegment * edge = fullViewEdgeSegmentsIterator->second;
			//UPDATE THE OCCLUSION METRIC FOR THIS EDGE
			std::map<int, std::pair<int, SegmentsGraph::SurfaceSegment*>> * connectedSurfaceSegments = edge->connections;
			for (std::map<int, std::pair<int, SegmentsGraph::SurfaceSegment*>>::iterator connectedSurfaceSegmentsIterator = connectedSurfaceSegments->begin();
				connectedSurfaceSegmentsIterator != connectedSurfaceSegments->end(); connectedSurfaceSegmentsIterator++)
			{
				//These are the surface segments connected
				int surfaceId = connectedSurfaceSegmentsIterator->first;
				//Check if there is a connection between surfaces ... that are visible in this partial view.
				
				mess.str(std::string("")); mess << "Checking visibility for segment with ID " << surfaceId;
				(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Warning);
				
				if (fullViewSurfaceSegmentsIterator->first != surfaceId && IsSegmentVisible(surfaceId, segmentsMap))
				{

					mess.str(std::string("")); mess << "Checking connection between full view segment ids : " << fullViewSurfaceSegmentsIterator->first << " and " << surfaceId;
					(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Error);
					
					std::vector<int> mappedSourceSegmentId; std::vector<int>mappedSinkSegmentId; 
					//Compute the mappings of these surface ids in the partial view. Could be more than one if surface is decomposed.
					for (std::map<std::pair<int,int>, int>::iterator keyIterator = segmentsMap.begin(); keyIterator != segmentsMap.end(); keyIterator++)
					{
						if (keyIterator->first.first == fullViewSurfaceSegmentsIterator->first) mappedSourceSegmentId.push_back(keyIterator->first.second);
						if (keyIterator->first.first == surfaceId) mappedSinkSegmentId.push_back(keyIterator->first.second);
					}

					mess.str(std::string(""));	mess << "FullView Source Maps to " << mappedSourceSegmentId.size() << " segments in partial view.";
												mess << "\nFullView Sink Maps to " << mappedSinkSegmentId.size() << " segments in partial view.";
					(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Warning);
										
					//Now go through the mapped source segment id in the partial view and check if there are connections to the mapped sink segments
					match = 0;
					for (int i=0; i<mappedSourceSegmentId.size(); i++)
					{
						//Determine the surface segments which are connected to this segment in the partial view scan.
						partialViewEdgesConnected = p_viewPCS->m_segmentsGraph->SurfaceSegmentWithId(mappedSourceSegmentId[i])->connections;
						
						mess.str(std::string("")); mess << "Checking connection between full view segment ids : " << fullViewSurfaceSegmentsIterator->first << " and " << surfaceId;
						(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Error);		
						
						//m_segmentationLogsFile << partialViewEdgesConnected->size() << " edges are connected to " << mappedSourceSegmentId[i] << std::endl;
						for (std::map<int, SegmentsGraph::EdgeSegment*>::iterator partialViewEdgeSegmentsIterator = partialViewEdgesConnected->begin(); 
																					partialViewEdgeSegmentsIterator != partialViewEdgesConnected->end(); 
																					partialViewEdgeSegmentsIterator++)
						{
							std::map<int, std::pair<int, SegmentsGraph::SurfaceSegment*>> * viewConnectedSurfaceSegments = partialViewEdgeSegmentsIterator->second->connections;

							mess.str(std::string("")); mess << "Edge with id " << partialViewEdgeSegmentsIterator->second->id << " has " << viewConnectedSurfaceSegments->size()  << " surface segment connected ";
							(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Warning);
												
							for (std::map<int, std::pair<int, SegmentsGraph::SurfaceSegment*>>::iterator conn_surfaces_iterator = viewConnectedSurfaceSegments->begin();
																											conn_surfaces_iterator != viewConnectedSurfaceSegments->end();
																											conn_surfaces_iterator++)
							{
								for (int j=0; j<mappedSinkSegmentId.size(); j++)
								{
									mess.str(std::string("")); mess << "Matching " << mappedSinkSegmentId[j] << " and " << conn_surfaces_iterator->first << " :" << match;
									(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Info);

									if (mappedSinkSegmentId[j] == conn_surfaces_iterator->first) match++;
								}
							}
						}
					}

					mess.str(std::string("")); mess << "Matched = " << match;
					(*(m_pEnvironment->GetLogger()))["Training"]->Write(mess.str(), LL_Error);			

					if (match==0)
					{
						// add to occluder list
						if (fullViewSurfaceSegmentsIterator->first < surfaceId) 
						{
							std::pair<int,int> occluded_surface_pair = std::pair<int,int>(fullViewSurfaceSegmentsIterator->first, surfaceId);
							if (m_edgeOcclusions->find(occluded_surface_pair) == m_edgeOcclusions->end()) (*m_edgeOcclusions)[occluded_surface_pair] = 0;
							(*m_edgeOcclusions)[occluded_surface_pair]++;
						}

					}
				}				
				//****************Check the next Surface Segment
			}
		}
	}

	//Finally enhance the internal state of those surfaces which got decomposed into a number of surface segments
	for (std::map<int, int>::iterator fullViewSurfaceSegmentsIterator = visibleSurfacesDecompositionCount.begin(); fullViewSurfaceSegmentsIterator != visibleSurfacesDecompositionCount.end(); 
		fullViewSurfaceSegmentsIterator++)
	{
		if (fullViewSurfaceSegmentsIterator->second > m_segmentsGraph->SurfaceSegmentWithId(fullViewSurfaceSegmentsIterator->first)->maxDecompositionCount)
			m_segmentsGraph->SurfaceSegmentWithId(fullViewSurfaceSegmentsIterator->first)->maxDecompositionCount = fullViewSurfaceSegmentsIterator->second;		
	}	
}

bool PointCloudSegmentation::IsSegmentVisible(int p_segmentId, std::map<std::pair<int,int>, int> &p_map)
{
	for (std::map<std::pair<int,int>, int>::iterator map_iterator = p_map.begin(); map_iterator!=p_map.end(); map_iterator++)
	{
		if (map_iterator->first.first == p_segmentId) return true;		
	}
	return false;
}

void PointCloudSegmentation::PrintStatistics()
{
	//m_segmentationLogsFile.open ("Output/Logs/OccluderEdgesLogs.txt", std::ios_base::app);
	//m_segmentationLogsFile << "PCS -" << m_pointCloud->GetName() << "- Data Sheet" << std::endl;
	//m_segmentationLogsFile << "Number of Surfaces = " << m_segmentsGraph->m_surfaceSegments->size() << std::endl;
	//m_segmentationLogsFile << "Number of Edges = " << m_segmentsGraph->m_edgeSegments->size() << std::endl;
	//m_segmentationLogsFile << "Number of Pruned Surfaces = " << m_segmentsGraph->m_prunedSurfaceSegments->size() << std::endl;	
	//m_segmentationLogsFile << "Max Visible Surfaces = " << m_maxSurfacesVisibleFromViews <<  std::endl;
	int max = 0;
	for (std::map<int, int>::iterator si = m_surfaceVisiblityCount.begin(); si != m_surfaceVisiblityCount.end(); si++)
	{
		//m_segmentationLogsFile << "SurfaceSegment ID: " << si->first << " is visible from " << si->second 
		//	<< " view points. Max Decomposed in " << m_segmentsGraph->SurfaceSegmentWithId(si->first)->maxDecompositionCount <<  std::endl;
		if (si->second > max) { max = si->second; m_mostVisibileSurface = si->first; }
	}
	//m_segmentationLogsFile << "Most Visible Surface = " << m_mostVisibileSurface <<  std::endl;
	//m_segmentationLogsFile << "Occlusion Information about " << m_edgeOcclusions->size() << " edges." <<  std::endl;
	for (std::map<std::pair<int,int>, int>::iterator edge_occlusion_iterator = m_edgeOcclusions->begin(); edge_occlusion_iterator != m_edgeOcclusions->end(); edge_occlusion_iterator++)
	{
		//m_segmentationLogsFile << "Occlusion between " << edge_occlusion_iterator->first.first << " and " << edge_occlusion_iterator->first.second << ". Score= " << edge_occlusion_iterator->second << std::endl;
	}
	//m_segmentationLogsFile.close();
}

//Iterative apply RanSAC to the surface segments in the segments graph. Return the number of primitives fitted
int PointCloudSegmentation::RANSACManager(SegmentsGraph::SurfaceSegment * p_segment, float p_errorTolerance, int p_stableCount, float p_fitRatio, bool p_edgeApplication)
{
	std::stringstream mess;
	bool done = false;
	bool fitsphere = false;
	bool fitcylinder = false;
	bool fitplane = true;
	int fit_count = 0;	
	std::vector<int> indices;
	for (int i = 0; i<p_segment->points->size(); i++) indices.push_back(p_segment->points->at(i)); 
	/*if (p_segment->id == 282) 
	{
		std::cout << "abc" << std::endl;
	}*/
	//First Try to fit a sphere, then a cylinder and finally a plane.		
	while (!done)
	{
		int indices_count = indices.size();
		if (fitsphere) 
		{ 
			/*try fitting a sphere*/
			if (RANSACFitSphere(p_errorTolerance, p_fitRatio, p_stableCount, p_segment, indices))
			{
				//Plane fitted to segment.
				if (indices.size() > indices_count/3 && indices.size() > 10) 
				{
					//and we should try fitting further spheres
					fit_count++;
				} 
				else
				{
					fitsphere = false;
				}
			} else
			{
				//we might consider inccreasing the errorTolerance or fitRatio. For Now just set the fitsphere variable to false
				fitsphere = false;
			}
		}
		if (fitcylinder) 
		{ 
			/*try fitting a cylinder*/ 
			if (RANSACFitCylinder(p_errorTolerance, p_fitRatio, p_stableCount, p_segment, indices))
			{
				//Plane fitted to segment.
				if (indices.size() > indices_count/3 && indices.size() > 10) 
				{
					//and we should try fitting further spheres
					fit_count++;
				}
				else 
				{
					fitcylinder = false;
				}
			} else
			{
				//we might consider increasing the errorTolerance or fitRatio. For now just set the fitcylinder variable to false
				fitcylinder = false;
			}
		}
		if (fitplane)
		{
			/*try fitting a plane*/
			int sp1, sp2, sp3; Vector3 sn;
			int pnt_count = indices.size();			
			if (RANSACFitPlane(p_errorTolerance, p_fitRatio, p_stableCount, p_segment, indices, sp1, sp2, sp3, sn))
			{
				//Plane fitted to segment.
				/****TESTING****/
				/*float d = Vector3::Dot(-sn, sp1);
				//iterate through the points in the patch and check whether they fit this plane
				float distance = 0;
				int nextIdx = 0;
				int crtfitcount = 0;
				for (int i=0; i<p_segment->points.size(); i++)
				{
					nextIdx = p_segment->points.at(i);
					distance = (m_pointCloud->m_pointList[nextIdx].m_vertex.Position.X * sn.X) +
								(m_pointCloud->m_pointList[nextIdx].m_vertex.Position.Y * sn.Y) +
								(m_pointCloud->m_pointList[nextIdx].m_vertex.Position.Z * sn.Z) + d;
					if (-p_errorTolerance <= distance && distance <= p_errorTolerance) crtfitcount++;					
				}
				std::cout << "*******************************************************************" << std::endl;
				std::cout << "[TEST RANSAC] Fitted = " << crtfitcount << std::endl;
				std::cout << "*******************************************************************" << std::endl;*/
				/**********************/

				fit_count++;
				mess.str(std::string("")); mess << "[RANSACManager] S::" << p_segment->id << ", PLANE FIT #Pnts:" << pnt_count - indices.size() << " #NotFitted:" << indices.size() << ", Threshold:" << indices_count/3;
				(*(m_pEnvironment->GetLogger()))["RANSAC"]->Write(mess.str(), LL_Error);
				m_pEnvironment->GetLogger()->Write(mess.str(), LL_Info);

				//if (p_segment->id == 334 || p_segment->id == 344)
				//{
				//	std::cout << "abc" << std::endl;
				//}

				if (indices.size() > 1000 || (indices.size() > indices_count/10 && indices.size() > 36)) 
				{
					//should try fitting the rest of the points.
				}
				else 
				{
					//std::cout << "[RANSAC Manager] NO PLANE FITTED - NotFitted:" << indices.size() << ", Threshold:" << indices_count/3 << std::endl;
					fitplane = false;
					done = true;   //This is the last primitive we try fitting. If this does not work break out of the loop
				}
			} else
			{
				//might consider increasing the errorTolerance or fitRatio. For now just set the fitplane variable to false
				fitplane = false;
				done = true; //This is the last primitive we try fitting. If this does not work break out of the loop
			}
		}
	}

	if (fit_count > 0)
	{		
		mess.str(std::string("")); mess << "[RANSACManager] DONE :: Fitted " << fit_count << " primitives.";
		(*(m_pEnvironment->GetLogger()))["RANSAC"]->Write(mess.str(), LL_Critical);					
						
		//Iterate over the internal primitives created				
		float buf = 0.01f;

		//Shift across internal segments any points which do not comply with the surface normal of the plane they are currently in, but match (included in plane + same surface normal) another internal surface.

		std::map<int, std::vector<int>> toAdd;
		std::map<int, std::vector<int>> toRemove;
		for (int i=0; i<fit_count; i++)
		{			
			SegmentsGraph::SurfaceSegment * crtInternalSegment = p_segment->internalSegGraph->SurfaceSegmentAtIndex(i);
			int originalPointCount = crtInternalSegment->points->size();			
			Vector3 crtInternalSegmentSurfaceNormal = crtInternalSegment->orientation;
			//std::cout << "CrtInternalSegmentsSurfaceNormal :: " << crtInternalSegmentSurfaceNormal.ToString() << std::endl;
			Vector3 crtPointNormal(0);
			VertexP crtPointVertex;
			float distance = 0;
			float surf_distance = 0;
			int in_count = 0;
			int out_count = 0;
			int move_count = 0;
			for (int j=0; j<crtInternalSegment->points->size(); j++)
			{
				crtPointNormal = m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_normal;				
				//std::cout << "DP->" << Vector3::AbsDot(crtPointNormal, crtInternalSegmentSurfaceNormal) << std::endl;
				if (Vector3::AbsDot(crtPointNormal, crtInternalSegmentSurfaceNormal) < 0.7f)
				{
					out_count++;
					//Check the other surfaces
					bool shuffled = false;
					for (int k=0; k<fit_count; k++)
					{
						if (Vector3::AbsDot(crtPointNormal, p_segment->internalSegGraph->SurfaceSegmentAtIndex(k)->orientation) > 0.6)
						{
							crtPointVertex = m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_vertex;
							surf_distance = p_segment->internalSegGraph->SurfaceSegmentAtIndex(k)->plane_d;
							distance = (crtPointVertex.Position.X * crtInternalSegmentSurfaceNormal.X) + (crtPointVertex.Position.Y * crtInternalSegmentSurfaceNormal.Y) + (crtPointVertex.Position.Z * crtInternalSegmentSurfaceNormal.Z) - surf_distance;														
							if (-p_errorTolerance <= distance && distance <= p_errorTolerance)
							{
								//Shift point to new segment
								toAdd[k].push_back(crtInternalSegment->points->at(j));
								toRemove[i].push_back(crtInternalSegment->points->at(j));
								shuffled = true;
								move_count++;
							}
						}
					}

					if (!shuffled)
					{
						toRemove[i].push_back(crtInternalSegment->points->at(j));
						m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_segmentId2 = -1;
					}
				}
				else { in_count++; }				
			}
			//std::cout << "ID::" << crtInternalSegment->id << " i:" << in_count << " o:" << out_count << " m:" << move_count << std::endl;
		}

		for (int i=0; i<fit_count; i++) std::cout << "Bef Id::" << p_segment->internalSegGraph->SurfaceSegmentAtIndex(i)->id << " Size::" << p_segment->internalSegGraph->SurfaceSegmentAtIndex(i)->points->size() << std::endl;

		SegmentsGraph::SurfaceSegment * crtInternalSegment;
		for (auto rem : toRemove)
		{
			crtInternalSegment = p_segment->internalSegGraph->SurfaceSegmentAtIndex(rem.first);
			std::cout << "Removing from Surface Id::" << crtInternalSegment->id << ", " << rem.second.size() << " points" << std::endl;
			//Build a temp map with the indices to remove
			std::map <int, bool> r;
			for (auto idx : rem.second) r[idx] = true;
			//Make a temp list of points.
			std::vector<int> tmpIndices;
			for (auto idx : *(crtInternalSegment->points)) tmpIndices.push_back(idx);
			//clear the internal list of points.
			crtInternalSegment->points->clear();
			//Re-Build
			for (auto idx : tmpIndices) if (r.find(idx) == r.end()) crtInternalSegment->points->push_back(idx);
		}

		for (auto add : toAdd)
		{
			std::cout << "Adding to Surface Id::" << add.first << ", " << add.second.size() << " points" << std::endl;
			crtInternalSegment = p_segment->internalSegGraph->SurfaceSegmentAtIndex(add.first);
			for (auto idx : add.second) 
			{
				crtInternalSegment->points->push_back(idx);
				m_pointCloud->m_pointList[idx].m_segmentId2 = crtInternalSegment->id;
			}
		}

		//for (int i=0; i<fit_count; i++) std::cout << "Aft Id::" << p_segment->internalSegGraph->SurfaceSegmentAtIndex(i)->id << " Size::" << p_segment->internalSegGraph->SurfaceSegmentAtIndex(i)->points->size() << std::endl; 

		//TODO
		//Try to establish connectibity between the newly generated segments using either
		// 1) Label as edges those points which were not included in primitive objects - Not very effective if p_errortolerance is high
		// 2) Label as edges those points which overlap different primitive segments i.e. - Will implement this method.
		//	- first establish an OBB for each plane segment created. 
		//	- for each point in the level 0 surface segment check whether the point falls in two different primitives.
		for (int i=0; i<fit_count; i++)
		{
			SegmentsGraph::SurfaceSegment * crtInternalSegment = p_segment->internalSegGraph->SurfaceSegmentAtIndex(i);
			Eigen::Matrix3f covariance = Eigen::Matrix3f();				
			Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver;
			if (crtInternalSegment->shape_primitive == SegmentsGraph::plane) 
			{
				int numOfPoints = crtInternalSegment->points->size();
				float c00, c11, c22, c01, c02, c12;
				c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;						
				Vector3 median = Vector3(0);								
				for (int j=0; j<numOfPoints; j++)
				{					
					median.X += m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_vertex.Position[0];
					median.Y += m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_vertex.Position[1];
					median.Z += m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_vertex.Position[2];							
				}
				median /= numOfPoints;

				//Now calculate diagonal of covariance matrix
				for (int j=0; j<numOfPoints; j++) 
				{			
					c00 += (m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_vertex.Position[0] - median.X) * (m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_vertex.Position[0] - median.X);
					c11 += (m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_vertex.Position[1] - median.Y) * (m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_vertex.Position[1] - median.Y);
					c22 += (m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_vertex.Position[2] - median.Z) * (m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_vertex.Position[2] - median.Z);
					c01 += (m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_vertex.Position[0] - median.X) * (m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_vertex.Position[1] - median.Y);
					c02 += (m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_vertex.Position[0] - median.X) * (m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_vertex.Position[2] - median.Z);
					c12 += (m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_vertex.Position[1] - median.Y) * (m_pointCloud->m_pointList[crtInternalSegment->points->at(j)].m_vertex.Position[2] - median.Z);		
				}				

				covariance << c00 / numOfPoints, c01 / numOfPoints, c02 / numOfPoints, 
							c01 / numOfPoints, c11 / numOfPoints, c12 / numOfPoints, 
							c02 / numOfPoints, c12 / numOfPoints, c22 / numOfPoints;
				
				eigensolver.compute(covariance);			
				//std::cout << "[Eigenvectors] " << eigensolver.eigenvectors().col(0) << " " << eigensolver.eigenvectors().col(1) << " " << eigensolver.eigenvectors().col(2) << " " << std::endl; 

				SegmentsGraph::OBB * obb = new SegmentsGraph::OBB();

				obb->obb_0 = Vector3(eigensolver.eigenvectors().col(0)[0], eigensolver.eigenvectors().col(0)[1], eigensolver.eigenvectors().col(0)[2]);
				obb->obb_1 = Vector3(eigensolver.eigenvectors().col(1)[0], eigensolver.eigenvectors().col(1)[1], eigensolver.eigenvectors().col(1)[2]);
				obb->obb_2 = Vector3(eigensolver.eigenvectors().col(2)[0], eigensolver.eigenvectors().col(2)[1], eigensolver.eigenvectors().col(2)[2]);

				//Set Orientation for segment
				crtInternalSegment->orientation = obb->obb_0;

				//Now move all the points in the coordinate space of the OBB
				std::vector<Vector3> OBBpoints;
				for (int i=0; i<numOfPoints; i++) OBBpoints.push_back(Vector3(Vector3::Dot(m_pointCloud->m_pointList[crtInternalSegment->points->at(i)].m_vertex.Position, obb->obb_0),
																				Vector3::Dot(m_pointCloud->m_pointList[crtInternalSegment->points->at(i)].m_vertex.Position, obb->obb_1), 
																				Vector3::Dot(m_pointCloud->m_pointList[crtInternalSegment->points->at(i)].m_vertex.Position, obb->obb_2)));

				float obb_0_d_min = 1000000.0f; float obb_0_d_max = -1000000.0f;
				float obb_1_d_min = 1000000.0f; float obb_1_d_max = -1000000.0f;
				float obb_2_d_min = 1000000.0f; float obb_2_d_max = -1000000.0f;

				for (int i=0; i<OBBpoints.size(); i++) 
				{
					if (OBBpoints[i].X > obb_0_d_max) obb_0_d_max = OBBpoints[i].X;
					if (OBBpoints[i].Y > obb_1_d_max) obb_1_d_max = OBBpoints[i].Y;
					if (OBBpoints[i].Z > obb_2_d_max) obb_2_d_max = OBBpoints[i].Z;

					if (OBBpoints[i].X < obb_0_d_min) obb_0_d_min = OBBpoints[i].X;
					if (OBBpoints[i].Y < obb_1_d_min) obb_1_d_min = OBBpoints[i].Y;
					if (OBBpoints[i].Z < obb_2_d_min) obb_2_d_min = OBBpoints[i].Z;
				}

				obb->p_0_min = Plane(obb->obb_0, obb_0_d_min - buf);
				obb->p_0_max = Plane(obb->obb_0, obb_0_d_max + buf);
				obb->p_1_min = Plane(obb->obb_1, obb_1_d_min - buf);
				obb->p_1_max = Plane(obb->obb_1, obb_1_d_max + buf);
				obb->p_2_min = Plane(obb->obb_2, obb_2_d_min - buf);
				obb->p_2_max = Plane(obb->obb_2, obb_2_d_max + buf);

				(*(p_segment->internalSegGraph->m_planePrimitivesOBB))[crtInternalSegment->id] = obb;
				crtInternalSegment->obb = *obb;				
			} //endif check plane
		}

		//All plane primitive OBBs are now computed. now Generate graph, i.e. connections between the segments.
		//Iterate over all points and check whether they fit in more than one obb - only if there are at least two plane primitive obbs
		if (p_segment->internalSegGraph->m_planePrimitivesOBB->size() > 1)
		{
			mess.str(std::string("")); mess << "[RANSACManager] Ransac on Internal :: Fitted " << fit_count << " primitives.";
			(*(m_pEnvironment->GetLogger()))["RANSAC"]->Write(mess.str(), LL_Critical);
			//Enlarge the OBB volumes in order to allow for overlaps between them. Reduce them when done.
			float buf = 0.2f;
			for (p_segment->internalSegGraph->m_planePrimitivesOBBIterator = p_segment->internalSegGraph->m_planePrimitivesOBB->begin(); 
						p_segment->internalSegGraph->m_planePrimitivesOBBIterator != p_segment->internalSegGraph->m_planePrimitivesOBB->end();
						p_segment->internalSegGraph->m_planePrimitivesOBBIterator++)
			{
				p_segment->internalSegGraph->m_planePrimitivesOBBIterator->second->p_0_min.Distance -= buf;
				p_segment->internalSegGraph->m_planePrimitivesOBBIterator->second->p_0_max.Distance += buf;
				p_segment->internalSegGraph->m_planePrimitivesOBBIterator->second->p_1_min.Distance -= buf;
				p_segment->internalSegGraph->m_planePrimitivesOBBIterator->second->p_1_max.Distance += buf;
				p_segment->internalSegGraph->m_planePrimitivesOBBIterator->second->p_2_min.Distance -= buf;
				p_segment->internalSegGraph->m_planePrimitivesOBBIterator->second->p_2_max.Distance += buf;				
			}

			std::map<std::string, int> seg_connectivity;
			Vector3 test_point_pos;			
			CloudPoint test_point(0,0,0);
			bool inOBB = false;
			for (int j=0; j<p_segment->points->size(); j++) 
			{
				test_point = m_pointCloud->m_pointList[p_segment->points->at(j)];
				test_point_pos = test_point.m_vertex.Position;
				for (p_segment->internalSegGraph->m_planePrimitivesOBBIterator = p_segment->internalSegGraph->m_planePrimitivesOBB->begin(); 
						p_segment->internalSegGraph->m_planePrimitivesOBBIterator != p_segment->internalSegGraph->m_planePrimitivesOBB->end();
						p_segment->internalSegGraph->m_planePrimitivesOBBIterator++)
				{
					int crtSegmentId = p_segment->internalSegGraph->m_planePrimitivesOBBIterator->first;
					SegmentsGraph::OBB * crtOBB = p_segment->internalSegGraph->m_planePrimitivesOBBIterator->second;
					inOBB = false;
					if (crtOBB->p_0_min.GetSide(test_point_pos) == Plane::Side_Positive)
						if (crtOBB->p_0_max.GetSide(test_point_pos) == Plane::Side_Negative)
							if (crtOBB->p_1_min.GetSide(test_point_pos) == Plane::Side_Positive)
								if (crtOBB->p_1_max.GetSide(test_point_pos) == Plane::Side_Negative)
									if (crtOBB->p_2_min.GetSide(test_point_pos) == Plane::Side_Positive)
										if (crtOBB->p_2_max.GetSide(test_point_pos) == Plane::Side_Negative)
											inOBB = true;		
					if (inOBB) 
					{
						bool segment_prev_set = true;
						//Check first if this point already has an internal segmentId set. If it doesn't have one set it and move out from this scope.
						if (test_point.m_segmentId2 == -1) { test_point.m_segmentId2 = crtSegmentId; segment_prev_set=false; }
						//Now check if the segment currently assigned is different from the one currently being checked
						if (segment_prev_set)
						{
							if (test_point.m_segmentId2 == crtSegmentId)
							{
								//the point is in it's obb. Not much we can do
							}
							else
							{
								//this point lies in at least two obbs. Report this fact in the connections hashmap
								std::string key = "";
								if (test_point.m_segmentId2 < crtSegmentId) 
								{ 
									key.append(boost::lexical_cast<std::string, int>(test_point.m_segmentId2)); 
									key.append("-");
									key.append(boost::lexical_cast<std::string, int>(crtSegmentId)); 									
									if (seg_connectivity.find(key)==seg_connectivity.end()) seg_connectivity[key] = 0;
									seg_connectivity[key] += 1;
								}
							}
						}
					}
				}
			}
			//Connectivity map generated			
			//Restore to previous OBB volumes			
			for (p_segment->internalSegGraph->m_planePrimitivesOBBIterator = p_segment->internalSegGraph->m_planePrimitivesOBB->begin(); 
						p_segment->internalSegGraph->m_planePrimitivesOBBIterator != p_segment->internalSegGraph->m_planePrimitivesOBB->end();
						p_segment->internalSegGraph->m_planePrimitivesOBBIterator++)
			{
				p_segment->internalSegGraph->m_planePrimitivesOBBIterator->second->p_0_min.Distance += buf;
				p_segment->internalSegGraph->m_planePrimitivesOBBIterator->second->p_0_max.Distance -= buf;
				p_segment->internalSegGraph->m_planePrimitivesOBBIterator->second->p_1_min.Distance += buf;
				p_segment->internalSegGraph->m_planePrimitivesOBBIterator->second->p_1_max.Distance -= buf;
				p_segment->internalSegGraph->m_planePrimitivesOBBIterator->second->p_2_min.Distance += buf;
				p_segment->internalSegGraph->m_planePrimitivesOBBIterator->second->p_2_max.Distance -= buf;				
			}

			typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
			boost::char_separator<char> sep("-");
			int internalSegmentsCount = p_segment->internalSegGraph->SurfaceSegmentsCount()+1;
			//Create edge states between connected segments in the connectivity map. Then produce the structure graph
			for (std::map<std::string, int>::iterator conn_iter = seg_connectivity.begin(); conn_iter != seg_connectivity.end(); conn_iter++)
			{
				mess.str(std::string("")); mess << "[InternalGraph Connections Established] SegmentIds=" << conn_iter->first << " Points=" << conn_iter->second;
				(*(m_pEnvironment->GetLogger()))["RANSAC"]->Write(mess.str(), LL_Critical);				
				//Create an edge between these two segments (if one does not already exist
				tokenizer tokens(conn_iter->first, sep);
				tokenizer::iterator tok_iter = tokens.begin();				
				int seg_1 = boost::lexical_cast<int, std::string>(*tok_iter); ++tok_iter;
				int seg_2 = boost::lexical_cast<int, std::string>(*tok_iter);
				//Create an edge between seg_1 and seg_2
				p_segment->internalSegGraph->CreateNewEdgeSegment(internalSegmentsCount);				
				p_segment->internalSegGraph->AddConnection(seg_1, internalSegmentsCount);
				p_segment->internalSegGraph->AddConnection(seg_2, internalSegmentsCount);
				internalSegmentsCount+=1; //increment edge id
			}

			//Establish initial surfaces for Disjoint Segment Graphs.
			bool all_connected = false;
			std::map<int, bool> vmap;			
			for (std::map<int, SegmentsGraph::SurfaceSegment*>::iterator surfaceSegmentsIterator = p_segment->internalSegGraph->m_surfaceSegments->begin();
				surfaceSegmentsIterator!=p_segment->internalSegGraph->m_surfaceSegments->end(); surfaceSegmentsIterator++)
				vmap[surfaceSegmentsIterator->first] = false;

			int nextIdx = 1;
			while (!all_connected)
			{
				SegmentsGraph::SurfaceSegment * crt_segment = p_segment->internalSegGraph->SurfaceSegmentAtIndex(nextIdx);
				(*(p_segment->internalSegGraph->m_disjointSegmentsGraphs))[crt_segment->id] = crt_segment;
				std::vector<int> bfl = p_segment->internalSegGraph->BreadthFirstTraversal(crt_segment);
				vmap[crt_segment->id] = true;
				for (int i=0; i<bfl.size(); i++) vmap[bfl[i]] = true;
				//check if all got connected.
				int idx = -1;
				for (std::map<int, bool>::iterator vmap_iterator = vmap.begin(); vmap_iterator != vmap.end(); vmap_iterator++)
					if (vmap_iterator->second == false) idx = vmap_iterator->first;

				if (idx!=-1)
				{
					nextIdx = idx;
				}
				else
				{
					all_connected = true;
				}
			}

			mess.str(std::string("")); mess << "[Internal Segment Graph] Parent SegmentId: " << p_segment->id << " Surface Segments=" << p_segment->internalSegGraph->m_surfaceSegments->size();
			(*(m_pEnvironment->GetLogger()))["RANSAC"]->Write(mess.str(), LL_Critical);

			//Now compute structure graph - only if ransac is being applied on a surface segment. 
			if (!p_edgeApplication)
			{
				p_segment->internalStrGraph = ComputeSceneStructureGraphFromSceneSegmentsGraph(p_segment->internalSegGraph, m_pointCloud->GetWorldType());
				p_segment->shape_primitive = SegmentsGraph::compound;			

				mess.str(std::string("")); mess << "[Internal Structure Graph] SegmentId: " << p_segment->id << " Surface Nodes=" << p_segment->internalStrGraph->m_nodes->size() << std::endl;
				(*(m_pEnvironment->GetLogger()))["RANSAC"]->Write(mess.str(), LL_Critical);

				MergeInternalStates(true, p_segment, 2, 0.95f, 0.05f);
			}

		} //endif plane's OBB have been generated
		else 
		{
			if (p_segment->shape_primitive=SegmentsGraph::plane)   //only one internal segment
			{
				//we can promote the internal node to level 0. and remove it.
				std::vector<int> tmp;
				int crt_point;
				for (auto seg_points : *(p_segment->points))
				{
					bool remove = false;
					for (int i=0; i<indices.size(); i++)
					{
						if (indices[i] == seg_points)
						{
							remove = true;
							break;
						}
					}
					if (!remove) tmp.push_back(seg_points);
				}

				p_segment->points->clear();
				for (auto keep : tmp) p_segment->points->push_back(keep);

				//Prune Points
				for (auto in : indices) 				
				{				
					m_pointCloud->m_pointList.at(in).m_segmentId = -1;
					m_pointCloud->m_pointList.at(in).m_typeId = 7;
				}

				//Recompute OBB
				p_segment->obb.buffer = 0.5f;				
				float c00, c11, c22, c01, c02, c12;
				c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;						
				Eigen::Matrix3f covariance = Eigen::Matrix3f();				
				Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver;
				Vector3 median = Vector3(0);
				
				std::vector<int> all_points;
				for (int i=0; i<p_segment->points->size(); i++) all_points.push_back(p_segment->points->at(i));

				int numOfPoints = all_points.size();
				for (int j=0; j<numOfPoints; j++)
				{					
					median.X += m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[0];
					median.Y += m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[1];
					median.Z += m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[2];							
				}
				median /= numOfPoints;

				//Now calculate diagonal of covariance matrix
				for (int j=0; j<numOfPoints; j++) 
				{			
					c00 += (m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[0] - median.X) * (m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[0] - median.X);
					c11 += (m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[1] - median.Y) * (m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[1] - median.Y);
					c22 += (m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[2] - median.Z) * (m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[2] - median.Z);
					c01 += (m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[0] - median.X) * (m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[1] - median.Y);
					c02 += (m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[0] - median.X) * (m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[2] - median.Z);
					c12 += (m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[1] - median.Y) * (m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[2] - median.Z);		
				}				

				covariance << c00 / numOfPoints, c01 / numOfPoints, c02 / numOfPoints, 
							c01 / numOfPoints, c11 / numOfPoints, c12 / numOfPoints, 
							c02 / numOfPoints, c12 / numOfPoints, c22 / numOfPoints;
				
				eigensolver.compute(covariance);			
				//std::cout << "[Eiegenvectors] " << eigensolver.eigenvectors().col(0) << " " << eigensolver.eigenvectors().col(1) << " " << eigensolver.eigenvectors().col(2) << " " << std::endl; 

				p_segment->obb.obb_0 = Vector3(eigensolver.eigenvectors().col(0)[0], eigensolver.eigenvectors().col(0)[1], eigensolver.eigenvectors().col(0)[2]);				
				p_segment->obb.obb_1 = Vector3(eigensolver.eigenvectors().col(1)[0], eigensolver.eigenvectors().col(1)[1], eigensolver.eigenvectors().col(1)[2]);
				p_segment->obb.obb_2 = Vector3(eigensolver.eigenvectors().col(2)[0], eigensolver.eigenvectors().col(2)[1], eigensolver.eigenvectors().col(2)[2]);

				//Now move all the points in the coordinate space of the OBB
				std::vector<Vector3> OBBpoints;
				for (int i=0; i<numOfPoints; i++) OBBpoints.push_back(Vector3(Vector3::Dot(m_pointCloud->m_pointList[all_points[i]].m_vertex.Position, p_segment->obb.obb_0), 
																				Vector3::Dot(m_pointCloud->m_pointList[all_points[i]].m_vertex.Position, p_segment->obb.obb_1), 
																				Vector3::Dot(m_pointCloud->m_pointList[all_points[i]].m_vertex.Position, p_segment->obb.obb_2)));

				float obb_0_d_min = 1000000.0f; float obb_0_d_max = -1000000.0f;
				float obb_1_d_min = 1000000.0f; float obb_1_d_max = -1000000.0f;
				float obb_2_d_min = 1000000.0f; float obb_2_d_max = -1000000.0f;

				for (int i=0; i<OBBpoints.size(); i++) 
				{
					if (OBBpoints[i].X > obb_0_d_max) obb_0_d_max = OBBpoints[i].X;
					if (OBBpoints[i].Y > obb_1_d_max) obb_1_d_max = OBBpoints[i].Y;
					if (OBBpoints[i].Z > obb_2_d_max) obb_2_d_max = OBBpoints[i].Z;

					if (OBBpoints[i].X < obb_0_d_min) obb_0_d_min = OBBpoints[i].X;
					if (OBBpoints[i].Y < obb_1_d_min) obb_1_d_min = OBBpoints[i].Y;
					if (OBBpoints[i].Z < obb_2_d_min) obb_2_d_min = OBBpoints[i].Z;
				}

				p_segment->obb.p_0_min = Plane(p_segment->obb.obb_0, obb_0_d_min - p_segment->obb.buffer);
				p_segment->obb.p_0_max = Plane(p_segment->obb.obb_0, obb_0_d_max + p_segment->obb.buffer);
				p_segment->obb.p_1_min = Plane(p_segment->obb.obb_1, obb_1_d_min - p_segment->obb.buffer);
				p_segment->obb.p_1_max = Plane(p_segment->obb.obb_1, obb_1_d_max + p_segment->obb.buffer);
				p_segment->obb.p_2_min = Plane(p_segment->obb.obb_2, obb_2_d_min - p_segment->obb.buffer);
				p_segment->obb.p_2_max = Plane(p_segment->obb.obb_2, obb_2_d_max + p_segment->obb.buffer);
							
			}
			mess.str(std::string("")); mess << std::endl;
			(*(m_pEnvironment->GetLogger()))["RANSAC"]->Write(mess.str(), LL_Critical);
		}
	}
	return fit_count;
}

//If p_indices is empty then consider using all points in p_segmentId. 
//There's no need to pass the surface segment as Ransac will only be applied to level 0 surface segments in this pcs.
bool PointCloudSegmentation::RANSACFitPlane(float p_errorTolerance, float p_fitRatio, int p_stableCount, SegmentsGraph::SurfaceSegment * p_segment, std::vector<int> &p_indices, int &p_sp1, int &p_sp2, int &p_sp3, Vector3 &p_sn)
{
	Vector3 surfacePoint1 = Vector3::Zero;
	Vector3 surfacePoint2 = Vector3::Zero;
	Vector3 surfacePoint3 = Vector3::Zero;
	Vector3 surfaceNormal = Vector3::Zero;
	Vector3 surfaceMedian = Vector3::Zero;
	float d = 0.0f;
	float spd = 0.0f;
	Random r(7);

	std::stringstream mess;

	int crtSP1 = 0;
	int crtSP2 = 0;
	int crtSP3 = 0;

	int tries = 0;
	bool stable = false;
	int highestfitcount = 0;
	int stablecount = 0;
	std::vector<int> bestFittedPoints; //list storing best fitted indices so far from the point cloud
	std::vector<int> bestNotFittedPoints; //list storing the rest of the indices i.e. not fitted points
	std::vector<int> crtFittedPoints;
	std::vector<int> crtNotFittedPoints;

	SegmentsGraph::SurfaceSegment* activeSurfaceSegment = p_segment;
				
	std::vector<int> patchPoints;
	if (p_indices.size() == 0) 
	{
		for (int i=0; i<activeSurfaceSegment->points->size(); i++)
		{
			patchPoints.push_back(activeSurfaceSegment->points->at(i)); 
		}
	}
	else 
	{
		for (int i=0; i<p_indices.size(); i++)
		{
			patchPoints.push_back(p_indices.at(i)); 
		}
	}
	
	mess.str(std::string("")); mess << "[RANSACFitPlane] Fitting SegmentID:" << activeSurfaceSegment->id << " with " << patchPoints.size() << " points ";
	(*(m_pEnvironment->GetLogger()))["RANSAC"]->Write(mess.str(), LL_Critical);	

	int usedSampleSelection = 0;
	bool validSamples = true;
	int crtfitcount = 0;
	int giveup = 0;

	float distance = 0;
	int nextIdx = 0;
	float x;
	Vector3 crt_vrt_pos;
	bool obb_checked = false;

	//exit when maxfittedcount is stable
	while (true)
	{
		int verticesInPatch = patchPoints.size();		
		validSamples = true;
		tries++;
		giveup++;
				
		//Select the three 'random' points		
		crtSP1 = patchPoints[r.Next(0, verticesInPatch - 1)];
		crtSP2 = patchPoints[r.Next(0, verticesInPatch - 1)];
		crtSP3 = patchPoints[r.Next(0, verticesInPatch - 1)];		

		surfacePoint1 = m_pointCloud->m_pointList[crtSP1].m_vertex.Position;
		surfacePoint2 = m_pointCloud->m_pointList[crtSP2].m_vertex.Position;
		surfacePoint3 = m_pointCloud->m_pointList[crtSP3].m_vertex.Position;					

		if (giveup > p_stableCount)
		{ 
			mess.str(std::string("")); mess << "[RANSACFitPlane] Giving UP !!! Breaking from Plane Fitting loop";
			(*(m_pEnvironment->GetLogger()))["RANSAC"]->Write(mess.str(), LL_Critical);
			return false; 
		}
		
		//check that three points chosen are not collinear using triangle inequality property
		if ( (Vector3::Distance(surfacePoint2, surfacePoint3) + 2.f) > (Vector3::Distance(surfacePoint1, surfacePoint2) + Vector3::Distance(surfacePoint2, surfacePoint3)) ) continue;
		//..and are not the same
		if ( surfacePoint1.Equals(surfacePoint2) ) continue;
		if ( surfacePoint2.Equals(surfacePoint3) ) continue;
		if ( surfacePoint1.Equals(surfacePoint3) ) continue;

		//Discard points which have very different normals.
		Vector3 surfacePoint1Normal = m_pointCloud->m_pointList[crtSP1].m_normal;
		Vector3 surfacePoint2Normal = m_pointCloud->m_pointList[crtSP2].m_normal;
		Vector3 surfacePoint3Normal = m_pointCloud->m_pointList[crtSP3].m_normal;
		surfacePoint1Normal.Normalize(); surfacePoint2Normal.Normalize(); surfacePoint3Normal.Normalize();
		float div1 = Vector3::AbsDot(surfacePoint1Normal, surfacePoint2Normal);
		float div2 = Vector3::AbsDot(surfacePoint1Normal, surfacePoint3Normal);
		if  (div1 < 0.5f) continue;   //Try points which exhibit a similar surface normal
		if  (div2 < 0.5f) continue;   //Try points which exhibit a similar surface normal
				
		giveup = 0; //Reset giveup flag
		
		//calculate surface normal and distance of candidate plane
		surfaceNormal = Vector3::Cross(surfacePoint3 - surfacePoint1, surfacePoint2 - surfacePoint1);
		surfaceNormal.Normalize();
		d = (surfacePoint1.X * surfaceNormal.X) + (surfacePoint1.Y * surfaceNormal.Y) + (surfacePoint1.Z * surfaceNormal.Z);
				
		//iterate through the points in the patch and check whether they fit this plane
		distance = 0;
		nextIdx = 0;
		crtfitcount = 0;
				
		for (int i=0; i<verticesInPatch; i++)
		{
			nextIdx = patchPoints[i];
			crt_vrt_pos = m_pointCloud->m_pointList[nextIdx].m_vertex.Position;
		
			distance = (crt_vrt_pos.X * surfaceNormal.X) + (crt_vrt_pos.Y * surfaceNormal.Y) + (crt_vrt_pos.Z * surfaceNormal.Z) -d;
			if (-p_errorTolerance <= distance && distance <= p_errorTolerance) crtfitcount++;
		}
						
		if (highestfitcount >= crtfitcount)
		{
			stablecount += 1;
			mess.str(std::string(""));
			mess << "[RANSACFitPlane Stable] Try " << tries << ": " << " Stable:" << stablecount << ", Crt Fitted:" << crtfitcount << ", Highest:" << highestfitcount;
			(*(m_pEnvironment->GetLogger()))["RANSAC"]->Write(mess.str(), LL_Info);
			if (stablecount == p_stableCount) stable = true;
		}
		else
		{
			stablecount = 0;
			bestFittedPoints.clear();
			bestNotFittedPoints.clear();			
			for (int i=0; i<verticesInPatch; i++)
			{
				nextIdx = patchPoints[i];
				crt_vrt_pos = m_pointCloud->m_pointList[nextIdx].m_vertex.Position;

				distance = (crt_vrt_pos.X * surfaceNormal.X) + (crt_vrt_pos.Y * surfaceNormal.Y) + (crt_vrt_pos.Z * surfaceNormal.Z) -d;				
				(-p_errorTolerance <= distance && distance <= p_errorTolerance) ? bestFittedPoints.push_back(nextIdx) : bestNotFittedPoints.push_back(nextIdx); 
			}
			highestfitcount = bestFittedPoints.size();											
			p_sp1 = crtSP1; p_sp2 = crtSP2; p_sp3 = crtSP3;
			p_sn = surfaceNormal;
			spd = d;
			if (highestfitcount == verticesInPatch) stable = true;
		}		
						
		if (stable)
		{			
			//check if the amount is good enough in order to generate a ransac plane using the _fitratio parameter			
			/***************************************/
			if (bestFittedPoints.size() > verticesInPatch * p_fitRatio)
			{											
				//Set the shape primitive of this (level0) surface segment to compound (might already have been set)				
				//Create a new surface segment with shape primitive set to plane storing indices to the fitted points and add to internal segments graph				
				if (activeSurfaceSegment->internalSegGraph==NULL) activeSurfaceSegment->internalSegGraph = new SegmentsGraph(this);
				int newSurfaceSegmentId = activeSurfaceSegment->internalSegGraph->CreateNewSurfaceSegment(SegmentsGraph::plane);
				activeSurfaceSegment->internalSegGraph->AddPoints(newSurfaceSegmentId, bestFittedPoints);
				activeSurfaceSegment->internalSegGraph->m_surfaceSegments->at(newSurfaceSegmentId)->orientation = p_sn;
				activeSurfaceSegment->internalSegGraph->m_surfaceSegments->at(newSurfaceSegmentId)->plane_d = spd;
				
				for (int i=0; i<bestFittedPoints.size(); i++) { m_pointCloud->m_pointList[bestFittedPoints[i]].m_segmentId2 = newSurfaceSegmentId; }
				
				p_indices.clear();
				for (int i=0; i<bestNotFittedPoints.size(); i++) p_indices.push_back(bestNotFittedPoints.at(i));
				
				if (activeSurfaceSegment->internalSegGraph->SurfaceSegmentsCount()==1)
				{
					activeSurfaceSegment->shape_primitive = SegmentsGraph::plane;
					activeSurfaceSegment->orientation = p_sn;
				}
				else
				if (activeSurfaceSegment->internalSegGraph->SurfaceSegmentsCount()>1)
				{
					activeSurfaceSegment->shape_primitive = SegmentsGraph::compound;
				}
				
				mess.str(std::string("")); mess << "[RANSACFitPlane] PlaneFITTED!! Iter:" << tries << " Stable:" << stablecount << ", Highest:" << highestfitcount;
				(*(m_pEnvironment->GetLogger()))["RANSAC"]->Write(mess.str(), LL_Error);				

				return true;
			}
			else
			{
				//the points cannot reasonably be fitted within a plane
				mess.str(std::string("")); mess << "[RANSACFitPlane] NO FIT - QUITTING!! BestFittedPoints Size=" << bestFittedPoints.size() << ", VerticesInPatch*p_fitRatio=" << verticesInPatch * p_fitRatio;
				(*(m_pEnvironment->GetLogger()))["RANSAC"]->Write(mess.str(), LL_Critical);
				
				return false;
			}
		}							
	}
}

//If p_indices is empty then consider using all points in p_segmentId
bool PointCloudSegmentation::RANSACFitSphere(float p_errorTolerance, float p_fitRatio, int p_stableCount, SegmentsGraph::SurfaceSegment * p_segment, std::vector<int> &p_indices)
{
	return false;
}

//If p_indices is empty then consider using all points in p_segmentId
bool PointCloudSegmentation::RANSACFitCylinder(float p_errorTolerance, float p_fitRatio, int p_stableCount, SegmentsGraph::SurfaceSegment * p_segment, std::vector<int> &p_indices)
{
	return false;
}

void PointCloudSegmentation::DrawSceneGraph(StructureGraph * graph, std::string p_filepath, float p_obb_overlap)
{
	std::string filepath = "Output/Graphs/"+boost::lexical_cast<std::string, int>(m_id)+ "/"+boost::lexical_cast<std::string, int>(m_id)+".str";

	if (p_obb_overlap < 1)
		filepath = "Output/Graphs/"+boost::lexical_cast<std::string, int>(m_id)+ "/"+boost::lexical_cast<std::string, int>(m_id)+"_"+ boost::lexical_cast<std::string, float>(p_obb_overlap) +".str";

	//First create a GraphViz file describing the graph
	std::cout << "[DrawSceneGraph] Drawing to " << filepath;
	std::ofstream graphfile;
	
	graphfile.open (filepath, std::ios::trunc);
	graphfile << "digraph G\n\t{\n";
	graphfile << "\t\tgraph [fontsize=8 fontname=\"Verdana\" compound=true];";
	graphfile << "\t\tnode [shape=oval fontsize=7 fontname=\"Verdana\"];\n";
	std::map<std::string, std::string> crtEdges;
	std::map<int, int>::iterator merged_nodes_iterator;
		
	std::vector<StructureGraph::Node*> withInternalGraph;

	//First write the state names with some statistics
	for (std::map<int, StructureGraph::Node*>::iterator nods = graph->m_nodes->begin(); nods!=graph->m_nodes->end(); nods++)
	{				
		if (nods->second->segment->shape_primitive==SegmentsGraph::compound)
		{
			withInternalGraph.push_back(nods->second);
		}
		else
		{	merged_nodes_iterator =  graph->m_mergedNodesMap->find(nods->first);
			//Show only nodes which have not been merged.
			if (merged_nodes_iterator == graph->m_mergedNodesMap->end())
			{
				if (nods->second->segment->shape_primitive==SegmentsGraph::plane)
				{
					if (graph->m_mergedNodesSeeds->find(nods->second->id) != graph->m_mergedNodesSeeds->end())
					{
						int m_count = graph->m_mergedNodesSeeds->find(nods->second->id)->second.size();
						graphfile << "\t\t" << nods->first << "\t" << " [shape=\"parallelogram\" label=\"" << nods->first << "<" << m_count  << ">\"" << " fillcolor=\"green\"" << "]\n";
					}
					else
					{
						//std::string materialId = boost::lexical_cast<std::string, int>(nods->second->segment->id % IPointCloud::GetNumberOfMaterials());
						//std::string materialTag = "Colour"; materialTag.append(materialId);						
						//material_group->GetByIndex(nods->second->segment->id % IPointCloud::GetNumberOfMaterials())->
						//graphfile << "\t\t" << nods->first << "\t" << " [shape=\"parallelogram\" style=\"filled\" fillcolor=\"" << nods->second->segment->id % IPointCloud::GetNumberOfMaterials() << "\" label=\"" << nods->first << "|" << nods->second->stats->point_count  << "\"]\n";
						graphfile << "\t\t" << nods->first << "\t" << " [shape=\"parallelogram\" label=\"" << nods->first << "|" << nods->second->stats->point_count  << "\"]\n";
					}
				}
				else
				{
					if (graph->m_mergedNodesSeeds->find(nods->second->id) != graph->m_mergedNodesSeeds->end())
					{
						int m_count = graph->m_mergedNodesSeeds->find(nods->second->id)->second.size();
						graphfile << "\t\t" << nods->first << "\t" << " [label=\"" << nods->first << "<" << m_count  << ">\"" << " fillcolor=\"green\"" << "]\n";
					}
					else
					{
						graphfile << "\t\t" << nods->first << "\t" << " [label=\"" << nods->first << "|" << nods->second->stats->point_count  << "\"]\n";
					}
				}
			}
		}
	}

	graphfile << "\n\n";

	//output as separate subgraphs compound nodes, i.e. those having more than one node.
	StructureGraph * tmpStrGraph;
	StructureGraph::Node * tmpStrGraphNode;
	std::map<std::string, bool> tmpEdgeMap;
	for (int i = 0; i<withInternalGraph.size(); i++)
	{
		graphfile << "\tsubgraph " << withInternalGraph.at(i)->id << "\n\t{\n";
		graphfile << "\t\tlabel = " << withInternalGraph.at(i)->id << "\n";
		graphfile << "\t\tnode [style=filled]\n";
		tmpStrGraph = withInternalGraph.at(i)->segment->internalStrGraph;
		tmpEdgeMap.clear();
		for (int j = 0; j<tmpStrGraph->m_nodes->size(); j++)
		{			
			tmpStrGraphNode = tmpStrGraph->m_nodes->at(j);
			for (std::map<int, std::pair<StructureGraph::EdgeStats*, StructureGraph::Node*>*>::iterator toVisitIterator = tmpStrGraphNode->edges->begin(); 
				toVisitIterator != tmpStrGraphNode->edges->end(); toVisitIterator++)								
			{				
				std::string key;
				if (tmpStrGraphNode->id > toVisitIterator->second->second->id)
				{
					key = boost::lexical_cast<std::string>(tmpStrGraphNode->id);
					key.append(":");
					key.append(boost::lexical_cast<std::string>(toVisitIterator->second->second->id));				
				} 
				else
				{
					key = boost::lexical_cast<std::string>(toVisitIterator->second->second->id);
					key.append(":");
					key.append(boost::lexical_cast<std::string>(tmpStrGraphNode->id));					
				}
				if (tmpEdgeMap.find(key) != tmpEdgeMap.end()) continue;
					
				tmpEdgeMap[key] = true;					
				graphfile << "\t\t" << withInternalGraph.at(i)->id << "." << tmpStrGraphNode->id << "->" << withInternalGraph.at(i)->id << "." << toVisitIterator->second->second->id << " [dir=none style=dotted]\n";
			}
		}
		graphfile << "\t}\n";
	}
	
	StructureGraph::Node * crtNode;
	std::string k1;
	std::string k2;
	std::string n1;
	std::string n2;

	std::map<int, int>::iterator merged_state_1;
	std::map<int, int>::iterator merged_state_2;

	StructureGraph::Node * crtNode_M;
	StructureGraph::Node * toVisit_M;	

	for (int i=0; i<graph->m_disjointStructureGraphs->Size(); i++)
	{
		//graphfile << "\tsubgraph cluster" << i << "\n\t{\n";
		Illumina::Core::List<int> * nodesList = graph->BreadthFirstTraversal(graph->m_disjointStructureGraphs->At(i));
		for (int j=0; j<nodesList->Size(); j++)
		{
			if (graph->m_nodes->find(nodesList->At(j)) != graph->m_nodes->end())
				crtNode = graph->m_nodes->at(nodesList->At(j));
			else continue;
			for (std::map<int, std::pair<StructureGraph::EdgeStats*, StructureGraph::Node*>*>::iterator toVisitIterator = crtNode->edges->begin(); toVisitIterator != crtNode->edges->end(); toVisitIterator++)		
			{				
				if (p_obb_overlap > 1) 
				{
						
				}
				else
				{
					if (toVisitIterator->second->first->obb_overlap < p_obb_overlap)
					{
						//std::cout << "OBB_OVERLAP JUMP [" << p_obb_overlap << "] --> " << toVisitIterator->second->first->obb_overlap << std::endl;
						continue;
					}	
					else 
					{
						//std::cout << "OBB_OVERLAP NOJUMP [" << p_obb_overlap << "] --> " << toVisitIterator->second->first->obb_overlap << std::endl;
					}
				}

				k1.clear();
				k2.clear();
				n1.clear();
				n2.clear();

				merged_state_1 = graph->m_mergedNodesMap->find(crtNode->id);
				merged_state_2 = graph->m_mergedNodesMap->find(toVisitIterator->first);

				crtNode_M = crtNode;
				toVisit_M = toVisitIterator->second->second;

				if (merged_state_1 != graph->m_mergedNodesMap->end()) crtNode_M = graph->m_nodes->at(merged_state_1->second);
				if (merged_state_2 != graph->m_mergedNodesMap->end()) toVisit_M = graph->m_nodes->at(merged_state_2->second);

				if (crtNode_M->id > toVisit_M->id)
				{
					n1 = boost::lexical_cast<std::string>(crtNode_M->id);
					if (crtNode_M->segment->internalStrGraph!=NULL) n1.append(".0");
					n2 = boost::lexical_cast<std::string>(toVisit_M->id);
					if (toVisit_M->segment->internalStrGraph!=NULL) n2.append(".0");
					k1 = n1.append("->"); k1.append(n2);
				}
				else
				{
					n1 = boost::lexical_cast<std::string>(crtNode_M->id);
					if (crtNode_M->segment->internalStrGraph!=NULL) n1.append(".0");
					n2 = boost::lexical_cast<std::string>(toVisit_M->id);
					if (toVisit_M->segment->internalStrGraph!=NULL) n2.append(".0");
					k1 = n2.append("->"); k1.append(n1);
				}			

				if (crtNode_M->segment->internalStrGraph!=NULL && toVisit_M->segment->internalStrGraph!=NULL)
				{
					k2.append(" [dir=none lhead="); k2.append(boost::lexical_cast<std::string>(crtNode_M->id)); 
					k2.append(", ltail="); k2.append(boost::lexical_cast<std::string>(toVisit_M->id));
				}

				if (crtNode_M->segment->internalStrGraph!=NULL && toVisit_M->segment->internalStrGraph==NULL)
				{
					k2.append(" [dir=none lhead="); k2.append(boost::lexical_cast<std::string>(crtNode_M->id));
				}

				if (toVisit_M->segment->internalStrGraph!=NULL && crtNode_M->segment->internalStrGraph==NULL)
				{
					k2.append(" [dir=none ltail="); k2.append(boost::lexical_cast<std::string>(toVisit_M->id));
				}

				if (toVisit_M->segment->internalStrGraph==NULL && crtNode_M->segment->internalStrGraph==NULL)
				{					
					k2.append(" [dir=none");					
				}								

				float dot = toVisitIterator->second->first->dot;
				dot = Maths::Abs(dot);
				//std::cout << crtNode->id << " <DOT-" << dot << "-DOT> " << toVisitIterator->second->second->id << std::endl;
				if (dot>0.96f && dot<1.04f)
				{
					//k2.append(" label="); 
					//k2.append(boost::lexical_cast<std::string>(dot)); 
					//k2.append("-"); 
					//k2.append(boost::lexical_cast<std::string>(toVisitIterator->second->first->obb_overlap));
					k2.append(" penwidth=4 color=\"black\"]");
				}
				else 
				{
					if (dot>-0.04f && dot<0.04f)
					{
						//k2.append(" label="); 
						//k2.append(boost::lexical_cast<std::string>(0)); 
						//k2.append("-"); 
						//k2.append(boost::lexical_cast<std::string>(toVisitIterator->second->first->strength));
						k2.append(" penwidth=3 color=\"blue\"]");
					} 
					else
					{
						k2.append(" label="); k2.append(boost::lexical_cast<std::string>(dot)); 
						//k2.append("-"); k2.append(boost::lexical_cast<std::string>(toVisitIterator->second->first->strength));
						k2.append(" penwidth=2 color=\"orange\"]");
					}
				}
				crtEdges[k1] = k2;						
			}
		}

		for (std::map<std::string, std::string>::iterator niter = crtEdges.begin(); niter != crtEdges.end(); niter++)
		{
			graphfile << "\t\t" << niter->first << " " << niter->second << "\n";
		}

		crtEdges.clear();  //clear the map for the next disjoint subgraph
		//graphfile << "\t}\n";	
	}
		
	graphfile << "}";
	graphfile.close();
	std::cout << " .. [done]" << std::endl;
}

void PointCloudSegmentation::DrawSegmentsGraph(SegmentsGraph * graph, std::string p_filepath)
{
	//First create a GraphViz file describing the graph
	typedef boost::tokenizer<boost::char_separator<char>> tokenizer;
	boost::char_separator<char> sep("/");
	tokenizer::iterator beg;
	
	/*std::string filename;
	tokenizer tok(p_filepath, sep);
	beg = tok.begin();
	//Discard all except for the last token
	while (beg != tok.end()) { filename = *beg; ++beg; }
		*/	
	std::string filepath = "Output/Graphs/"+boost::lexical_cast<std::string, int>(m_id)+ "/"+boost::lexical_cast<std::string, int>(m_id)+".seg";


	//First create a GraphViz file describing the graph
	std::cout << "[DrawSegmentsGraph] Drawing to " << filepath;
	std::ofstream graphfile;
	
	graphfile.open (filepath, std::ios::trunc);
	graphfile << "graph G\n\t{\n";
		
	//First write the surface state names with some statistics
	for (std::map<int, SegmentsGraph::SurfaceSegment*>::iterator si = graph->m_surfaceSegments->begin(); si!=graph->m_surfaceSegments->end(); si++)
	{
		graphfile << "\t\t" << si->first << " [label=" << graph->PointCountInSegment(si->first) << "]\n";
	}

	//Then write the edge state names with some statistics
	for (std::map<int, SegmentsGraph::EdgeSegment*>::iterator ei = graph->m_edgeSegments->begin(); ei!=graph->m_edgeSegments->end(); ei++)
	{
		graphfile << "\t\t" << ei->first << " [label=" << graph->PointCountInSegment(ei->first) << " shape=octagon]\n";
	}

	//Now print the connections from each edge state. 
	for (std::map<int, SegmentsGraph::EdgeSegment*>::iterator ei = graph->m_edgeSegments->begin(); ei!=graph->m_edgeSegments->end(); ei++)
	{
		graphfile << "subgraph cluster" << ei->first << "\n\t{\n";
		for (std::map<int, std::pair<int, SegmentsGraph::SurfaceSegment*>>::iterator stateIterator = ei->second->connections->begin(); stateIterator != ei->second->connections->end(); stateIterator++)
		{			
			graphfile << "\t\t" << ei->first << "--" << stateIterator->first << " [Label=" << stateIterator->second.first << "]\n";
		}
		graphfile << "\t\t}\n";	
	}
		
	graphfile << "}";
	graphfile.close();
	std::cout << " .. [done]" << std::endl;
}

//***************************************//
//***PointCloud File Output Function*****//
//***************************************//
void PointCloudSegmentation::WritePointsToBinaryFile(KDTreePointCloud &p_pointCloud, std::string _filepath)
{	
	const char * filename = _filepath.c_str();
	size_t pc_size = p_pointCloud.m_pointList.size();
	std::filebuf fbuf;
	fbuf.open(filename, std::ios_base::in | std::ios_base::out | std::ios_base::trunc | std::ios_base::binary);
	fbuf.pubseekoff((pc_size*3*sizeof(float))-sizeof(char), std::ios_base::beg);				
	fbuf.sputc(0);
	fbuf.close();

	boost::interprocess::file_mapping m_file(filename, boost::interprocess::read_write);
	boost::interprocess::mapped_region region(m_file, boost::interprocess::read_write);						    

	float * r_addr = (float*)region.get_address();			
	size_t r_size = region.get_size();				
	std::cout << "[PCSupport] Writing PC to File - MMF created and mapped to region. Size= " << r_size << std::endl;
	size_t idx = 0;
				
	for (int i = 0; i<pc_size-1; i++) {					
		r_addr[idx] = p_pointCloud.m_pointList[i].m_vertex.Position.X; idx++;
		r_addr[idx] = p_pointCloud.m_pointList[i].m_vertex.Position.Y; idx++;
		r_addr[idx] = p_pointCloud.m_pointList[i].m_vertex.Position.Z; idx++;
	}

	region.flush();
}

void PointCloudSegmentation::WriteToBPCSFile(std::string _filepath)
{	
	std::string pcsPath = _filepath+boost::lexical_cast<std::string, int>(m_id)+".bpcs";
	const char * filename = pcsPath.c_str();
	//Determine Size ... structure graph + segments graph
	
	std::filebuf fbuf;
	fbuf.open(filename, std::ios_base::in | std::ios_base::out | std::ios_base::trunc | std::ios_base::binary);
	//float size_of_record = (3*sizeof(float)) + sizeof(int) + sizeof(int) + sizeof(char) + sizeof(char);

	int sizeofFile = (sizeof(int) * 3);
	sizeofFile += m_edgeOcclusions->size() * (sizeof(int) * 3);
	sizeofFile += sizeof(int);

	fbuf.pubseekoff( sizeofFile - sizeof(char), std::ios_base::beg);
	fbuf.sputc(0);
	fbuf.close();

	boost::interprocess::file_mapping m_file(filename, boost::interprocess::read_write);
	boost::interprocess::mapped_region region(m_file, boost::interprocess::read_write);						    

	int * r_addr_i = (int*)region.get_address();
	size_t r_size = region.get_size();				
	std::cout << "[PointCloudSegmentation] Writing to Binary File " << pcsPath << ". Size= " << r_size << std::endl;	
	
	*r_addr_i = m_id; r_addr_i++;
	*r_addr_i = m_maxSurfacesVisibleFromViews; r_addr_i++;
	*r_addr_i = m_mostVisibileSurface; r_addr_i++;

	//Write Occlusion information
	//std::map<std::pair<int,int>, int> * m_edgeOcclusions
	*r_addr_i = m_edgeOcclusions->size(); r_addr_i++;
	for (auto occ : *m_edgeOcclusions)
	{
		*r_addr_i = occ.first.first; r_addr_i++;
		*r_addr_i = occ.first.second; r_addr_i++;
		*r_addr_i = occ.second; r_addr_i++;
	}	
	
	m_pointCloud->WriteToBKD3File(_filepath+boost::lexical_cast<std::string, int>(m_id), false, (MaterialGroup*) m_pEngineKernel->GetMaterialManager()->RequestInstance("PCMaterial"));
	m_segmentsGraph->WriteToBSEGFile(_filepath+boost::lexical_cast<std::string, int>(m_id));
	m_structureGraph->WriteToBSTRFile(_filepath+boost::lexical_cast<std::string, int>(m_id));
	*r_addr_i = m_featureDescriptionGraphs->size(); r_addr_i++;
	for (auto fg : *m_featureDescriptionGraphs)
	{		
		fg->WriteToBFDSGFile(_filepath+boost::lexical_cast<std::string, int>(m_id)+"_"+boost::lexical_cast<std::string, int>(fg->initial_node_id));
		*r_addr_i = fg->initial_node_id; r_addr_i++;
	}

	region.flush();	

	std::cout << "[PointCloudSegmentation] Writing to Binary File " << pcsPath << ". [DONE]" << std::endl;
}

//Load and update structures of this PCS instance to those loaded from _filepath. 
//Loads m_id ... m_id should have been set before.
bool PointCloudSegmentation::ReadFromBPCSFile(std::string _filepath)
{			
	// Open .bpcs file
	std::string pcsPath = _filepath+boost::lexical_cast<std::string, int>(m_id)+".bpcs";
	boost::filesystem::path p (pcsPath, boost::filesystem::native);
	if ( !boost::filesystem::exists( p ) )
	{
		std::cerr << "[PointCloudSegmentation] Error : Couldn't open file '" << _filepath << "'" << std::endl;
		return false;
	}
	
	//we assume all points/file fit in main memory
	boost::interprocess::file_mapping m_file;		
	boost::interprocess::mapped_region *m_region;
	int * r_addr_i;
	float * r_addr_f;		
	
	const char * filename = pcsPath.c_str();
	m_file = boost::interprocess::file_mapping(filename, boost::interprocess::read_only);
	m_region = new boost::interprocess::mapped_region(m_file, boost::interprocess::read_only);					
	r_addr_f = (float*)m_region->get_address();
	r_addr_i = (int*)m_region->get_address();
	size_t r_size = m_region->get_size();						

	m_id = (int)*r_addr_i; r_addr_i++;

	std::cout << "[PointCloudSegmentation] Reading Bin File " << pcsPath << ". M-ID=" << m_id << std::endl;	

	m_maxSurfacesVisibleFromViews = (int)*r_addr_i; r_addr_i++;
	m_mostVisibileSurface = (int)*r_addr_i; r_addr_i++;	
	
	m_pointCloud = new KDTreePointCloud(boost::lexical_cast<std::string, int>(m_id));
	m_pointCloud->ReadFromBKD3File(_filepath+boost::lexical_cast<std::string, int>(m_id));	

	m_segmentsGraph = new SegmentsGraph(this);
	m_segmentsGraph->ReadFromBSEGFile(_filepath+boost::lexical_cast<std::string, int>(m_id));

	m_structureGraph = new StructureGraph(m_segmentsGraph);
	m_structureGraph->ReadFromBSTRFile(_filepath+boost::lexical_cast<std::string, int>(m_id));

	int numberOfOcclusionsRecords = (int)*r_addr_i; r_addr_i++;
	m_edgeOcclusions = new std::map<std::pair<int,int>,int>();
	for (int i=0; i<numberOfOcclusionsRecords; i++)
	{
		std::pair<int,int> p;
		p.first = (int)*r_addr_i; r_addr_i++;
		p.second = (int)*r_addr_i; r_addr_i++;
		(*m_edgeOcclusions)[p] = (int)*r_addr_i; r_addr_i++;		
	}

	m_featureDescriptionGraphs = new std::vector<FeatureDescStructureGraph*>();
	int number_of_fdg = (int)*r_addr_i; r_addr_i++;
	for (int i=0; i<number_of_fdg; i++)
	{
		int fg_id = (int)*r_addr_i; r_addr_i++;
		FeatureDescStructureGraph * fdsg = new FeatureDescStructureGraph(fg_id, this);
		fdsg->ReadFromBFDSGFile(_filepath+boost::lexical_cast<std::string, int>(m_id)+"_"+boost::lexical_cast<std::string, int>(fg_id));
		m_featureDescriptionGraphs->push_back(fdsg);
		fdsg->m_pcs = this;		
	}

	m_pointCloud->SetPCSId(m_id);			

	delete m_region;
	std::cout << "[PointCloudSegmentation] Loaded KdTree=" << m_pointCloud->GetSize() << "pnts, SeG=" << m_segmentsGraph->m_surfaceSegments->size() << "surfaces, StG=" << m_structureGraph->m_nodes->size() << " nodes." << std::endl;	
	if (m_featureDescriptionGraphs->size() > 0) { std::cout << "[PointCloudSegmentation] FDSG => "; for (auto fdg : *m_featureDescriptionGraphs) std::cout << fdg->initial_node_id << " "; std::cout << std::endl; }
	else { std::cout << "[PointCloudSegmentation] FDSG => 0 Loaded !!!!" << std::endl; }
	return true;
}

std::string PointCloudSegmentation::ToString()
{
	std::string to_string = "";
	to_string.append("ID: ");  to_string.append(boost::lexical_cast<std::string,int>(m_id)); 	
	to_string.append(" PointCount="); to_string.append(boost::lexical_cast<std::string,int>(m_pointCloud->m_pointList.size()));
	to_string.append(" SurfSegmentCount="); to_string.append(boost::lexical_cast<std::string,int>(m_segmentsGraph->m_surfaceSegments->size()));
	to_string.append(" FeatureGrCount="); to_string.append(boost::lexical_cast<std::string,int>(m_featureDescriptionGraphs->size()));
	return to_string;
}